mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-10-24 12:07:40 +00:00
Code Issues Releases Wiki Activity

Apply automated code formatting

Browse Source 
Documented at .clang-format
See http://clang.llvm.org/docs/ClangFormat.html and http://clang.llvm.org/docs/ClangFormatStyleOptions.html
This commit is contained in:
Carles Fernandez
2018-03-03 02:03:39 +01:00
parent c9ac8c78af
commit 34f24562cf
902 changed files with 36939 additions and 34573 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
src/algorithms/libs/byte_x2_to_complex_byte.cc
View File

@@ -40,10 +40,9 @@ byte_x2_to_complex_byte_sptr make_byte_x2_to_complex_byte()
}
byte_x2_to_complex_byte::byte_x2_to_complex_byte() : sync_block("byte_x2_to_complex_byte",
gr::io_signature::make (2, 2, sizeof(int8_t)), // int8_t, defined in stdint.h and included in volk.h (signed char)
gr::io_signature::make (1, 1, sizeof(lv_8sc_t))) // lv_8sc_t is a Volk's typedef for std::complex<signed char>
gr::io_signature::make(2, 2, sizeof(int8_t)), // int8_t, defined in stdint.h and included in volk.h (signed char)
gr::io_signature::make(1, 1, sizeof(lv_8sc_t))) // lv_8sc_t is a Volk's typedef for std::complex<signed char>
{
const int alignment_multiple = volk_get_alignment() / sizeof(lv_8sc_t);
set_alignment(std::max(1, alignment_multiple));
@@ -51,8 +50,8 @@ byte_x2_to_complex_byte::byte_x2_to_complex_byte() : sync_block("byte_x2_to_comp
int byte_x2_to_complex_byte::work(int noutput_items,
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items)
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items)
{
const int8_t *in0 = reinterpret_cast<const int8_t *>(input_items[0]);
const int8_t *in1 = reinterpret_cast<const int8_t *>(input_items[1]);
@@ -60,7 +59,7 @@ int byte_x2_to_complex_byte::work(int noutput_items,
// This could be put into a volk kernel
int8_t real_part;
int8_t imag_part;
for(int number = 0; number < noutput_items; number++)
for (int number = 0; number < noutput_items; number++)
{
// lv_cmake(r, i) defined at volk/volk_complex.h
real_part = *in0++;

5
src/algorithms/libs/byte_x2_to_complex_byte.h
View File

@@ -49,12 +49,13 @@ class byte_x2_to_complex_byte : public gr::sync_block
{
private:
friend byte_x2_to_complex_byte_sptr make_byte_x2_to_complex_byte();
public:
byte_x2_to_complex_byte();
int work(int noutput_items,
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items);
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items);
};
#endif

9
src/algorithms/libs/complex_byte_to_float_x2.cc
View File

@@ -40,10 +40,9 @@ complex_byte_to_float_x2_sptr make_complex_byte_to_float_x2()
}
complex_byte_to_float_x2::complex_byte_to_float_x2() : sync_block("complex_byte_to_float_x2",
gr::io_signature::make (1, 1, sizeof(lv_8sc_t)), // lv_8sc_t is a Volk's typedef for std::complex<signed char>
gr::io_signature::make (2, 2, sizeof(float)))
gr::io_signature::make(1, 1, sizeof(lv_8sc_t)), // lv_8sc_t is a Volk's typedef for std::complex<signed char>
gr::io_signature::make(2, 2, sizeof(float)))
{
const int alignment_multiple = volk_get_alignment() / sizeof(float);
set_alignment(std::max(1, alignment_multiple));
@@ -51,8 +50,8 @@ complex_byte_to_float_x2::complex_byte_to_float_x2() : sync_block("complex_byte_
int complex_byte_to_float_x2::work(int noutput_items,
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items)
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items)
{
const lv_8sc_t *in = reinterpret_cast<const lv_8sc_t *>(input_items[0]);
float *out0 = reinterpret_cast<float *>(output_items[0]);

5
src/algorithms/libs/complex_byte_to_float_x2.h
View File

@@ -49,12 +49,13 @@ class complex_byte_to_float_x2 : public gr::sync_block
{
private:
friend complex_byte_to_float_x2_sptr make_complex_byte_to_float_x2();
public:
complex_byte_to_float_x2();
int work(int noutput_items,
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items);
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items);
};
#endif

11
src/algorithms/libs/complex_float_to_complex_byte.cc
View File

@@ -40,10 +40,9 @@ complex_float_to_complex_byte_sptr make_complex_float_to_complex_byte()
}
complex_float_to_complex_byte::complex_float_to_complex_byte() : sync_block("complex_float_to_complex_byte",
gr::io_signature::make (1, 1, sizeof(gr_complex)),
gr::io_signature::make (1, 1, sizeof(lv_8sc_t))) // lv_8sc_t is a Volk's typedef for std::complex<signed char>
gr::io_signature::make(1, 1, sizeof(gr_complex)),
gr::io_signature::make(1, 1, sizeof(lv_8sc_t))) // lv_8sc_t is a Volk's typedef for std::complex<signed char>
{
const int alignment_multiple = volk_gnsssdr_get_alignment() / sizeof(lv_8sc_t);
set_alignment(std::max(1, alignment_multiple));
@@ -51,11 +50,11 @@ complex_float_to_complex_byte::complex_float_to_complex_byte() : sync_block("com
int complex_float_to_complex_byte::work(int noutput_items,
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items)
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items)
{
const gr_complex *in = reinterpret_cast<const gr_complex *>(input_items[0]);
lv_8sc_t *out = reinterpret_cast<lv_8sc_t*>(output_items[0]);
lv_8sc_t *out = reinterpret_cast<lv_8sc_t *>(output_items[0]);
volk_gnsssdr_32fc_convert_8ic(out, in, noutput_items);
return noutput_items;
}

5
src/algorithms/libs/complex_float_to_complex_byte.h
View File

@@ -47,12 +47,13 @@ class complex_float_to_complex_byte : public gr::sync_block
{
private:
friend complex_float_to_complex_byte_sptr make_complex_float_to_complex_byte();
public:
complex_float_to_complex_byte();
int work(int noutput_items,
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items);
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items);
};
#endif

8
src/algorithms/libs/conjugate_cc.cc
View File

@@ -40,8 +40,8 @@ conjugate_cc_sptr make_conjugate_cc()
conjugate_cc::conjugate_cc() : gr::sync_block("conjugate_cc",
gr::io_signature::make (1, 1, sizeof(gr_complex)),
gr::io_signature::make (1, 1, sizeof(gr_complex)))
gr::io_signature::make(1, 1, sizeof(gr_complex)),
gr::io_signature::make(1, 1, sizeof(gr_complex)))
{
const int alignment_multiple = volk_get_alignment() / sizeof(gr_complex);
set_alignment(std::max(1, alignment_multiple));
@@ -49,8 +49,8 @@ conjugate_cc::conjugate_cc() : gr::sync_block("conjugate_cc",
int conjugate_cc::work(int noutput_items,
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items)
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items)
{
const gr_complex *in = reinterpret_cast<const gr_complex *>(input_items[0]);
gr_complex *out = reinterpret_cast<gr_complex *>(output_items[0]);

5
src/algorithms/libs/conjugate_cc.h
View File

@@ -48,12 +48,13 @@ class conjugate_cc : public gr::sync_block
{
private:
friend conjugate_cc_sptr make_conjugate_cc();
public:
conjugate_cc();
int work(int noutput_items,
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items);
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items);
};
#endif

8
src/algorithms/libs/conjugate_ic.cc
View File

@@ -40,8 +40,8 @@ conjugate_ic_sptr make_conjugate_ic()
conjugate_ic::conjugate_ic() : gr::sync_block("conjugate_ic",
gr::io_signature::make (1, 1, sizeof(lv_8sc_t)),
gr::io_signature::make (1, 1, sizeof(lv_8sc_t)))
gr::io_signature::make(1, 1, sizeof(lv_8sc_t)),
gr::io_signature::make(1, 1, sizeof(lv_8sc_t)))
{
const int alignment_multiple = volk_gnsssdr_get_alignment() / sizeof(lv_8sc_t);
set_alignment(std::max(1, alignment_multiple));
@@ -49,8 +49,8 @@ conjugate_ic::conjugate_ic() : gr::sync_block("conjugate_ic",
int conjugate_ic::work(int noutput_items,
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items)
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items)
{
const lv_8sc_t *in = reinterpret_cast<const lv_8sc_t *>(input_items[0]);
lv_8sc_t *out = reinterpret_cast<lv_8sc_t *>(output_items[0]);

5
src/algorithms/libs/conjugate_ic.h
View File

@@ -48,12 +48,13 @@ class conjugate_ic : public gr::sync_block
{
private:
friend conjugate_ic_sptr make_conjugate_ic();
public:
conjugate_ic();
int work(int noutput_items,
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items);
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items);
};
#endif

8
src/algorithms/libs/conjugate_sc.cc
View File

@@ -40,8 +40,8 @@ conjugate_sc_sptr make_conjugate_sc()
conjugate_sc::conjugate_sc() : gr::sync_block("conjugate_sc",
gr::io_signature::make (1, 1, sizeof(lv_16sc_t)),
gr::io_signature::make (1, 1, sizeof(lv_16sc_t)))
gr::io_signature::make(1, 1, sizeof(lv_16sc_t)),
gr::io_signature::make(1, 1, sizeof(lv_16sc_t)))
{
const int alignment_multiple = volk_gnsssdr_get_alignment() / sizeof(lv_16sc_t);
set_alignment(std::max(1, alignment_multiple));
@@ -49,8 +49,8 @@ conjugate_sc::conjugate_sc() : gr::sync_block("conjugate_sc",
int conjugate_sc::work(int noutput_items,
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items)
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items)
{
const lv_16sc_t *in = reinterpret_cast<const lv_16sc_t *>(input_items[0]);
lv_16sc_t *out = reinterpret_cast<lv_16sc_t *>(output_items[0]);

5
src/algorithms/libs/conjugate_sc.h
View File

@@ -48,12 +48,13 @@ class conjugate_sc : public gr::sync_block
{
private:
friend conjugate_sc_sptr make_conjugate_sc();
public:
conjugate_sc();
int work(int noutput_items,
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items);
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items);
};
#endif

9
src/algorithms/libs/cshort_to_float_x2.cc
View File

@@ -40,10 +40,9 @@ cshort_to_float_x2_sptr make_cshort_to_float_x2()
}
cshort_to_float_x2::cshort_to_float_x2() : sync_block("cshort_to_float_x2",
gr::io_signature::make (1, 1, sizeof(lv_16sc_t)), // lv_8sc_t is a Volk's typedef for std::complex<signed char>
gr::io_signature::make (2, 2, sizeof(float)))
gr::io_signature::make(1, 1, sizeof(lv_16sc_t)), // lv_8sc_t is a Volk's typedef for std::complex<signed char>
gr::io_signature::make(2, 2, sizeof(float)))
{
const int alignment_multiple = volk_get_alignment() / sizeof(lv_16sc_t);
set_alignment(std::max(1, alignment_multiple));
@@ -51,8 +50,8 @@ cshort_to_float_x2::cshort_to_float_x2() : sync_block("cshort_to_float_x2",
int cshort_to_float_x2::work(int noutput_items,
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items)
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items)
{
const lv_16sc_t *in = reinterpret_cast<const lv_16sc_t *>(input_items[0]);
float *out0 = reinterpret_cast<float *>(output_items[0]);

5
src/algorithms/libs/cshort_to_float_x2.h
View File

@@ -49,12 +49,13 @@ class cshort_to_float_x2 : public gr::sync_block
{
private:
friend cshort_to_float_x2_sptr make_cshort_to_float_x2();
public:
cshort_to_float_x2();
int work(int noutput_items,
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items);
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items);
};
#endif

67
src/algorithms/libs/galileo_e1_signal_processing.cc
View File

@@ -74,7 +74,7 @@ void galileo_e1_code_gen_int(int* _dest, char _Signal[3], signed int _prn)
void galileo_e1_sinboc_11_gen_int(int* _dest, int* _prn, unsigned int _length_out)
{
const unsigned int _length_in = Galileo_E1_B_CODE_LENGTH_CHIPS;
unsigned int _period = static_cast<unsigned int>( _length_out / _length_in );
unsigned int _period = static_cast<unsigned int>(_length_out / _length_in);
for (unsigned int i = 0; i < _length_in; i++)
{
for (unsigned int j = 0; j < (_period / 2); j++)
@@ -83,7 +83,7 @@ void galileo_e1_sinboc_11_gen_int(int* _dest, int* _prn, unsigned int _length_ou
}
for (unsigned int j = (_period / 2); j < _period; j++)
{
_dest[i * _period + j] = - _prn[i];
_dest[i * _period + j] = -_prn[i];
}
}
}
@@ -102,7 +102,7 @@ void galileo_e1_sinboc_61_gen_int(int* _dest, int* _prn, unsigned int _length_ou
}
for (unsigned int j = 1; j < _period; j += 2)
{
_dest[i * _period + j] = - _prn[i];
_dest[i * _period + j] = -_prn[i];
}
}
}
@@ -115,26 +115,26 @@ void galileo_e1_gen_float(float* _dest, int* _prn, char _Signal[3])
const float alpha = sqrt(10.0 / 11.0);
const float beta = sqrt(1.0 / 11.0);
int sinboc_11[12 * 4092]; // _codeLength not accepted by Clang
int sinboc_11[12 * 4092]; // _codeLength not accepted by Clang
int sinboc_61[12 * 4092];
galileo_e1_sinboc_11_gen_int(sinboc_11, _prn, _codeLength); //generate sinboc(1,1) 12 samples per chip
galileo_e1_sinboc_61_gen_int(sinboc_61, _prn, _codeLength); //generate sinboc(6,1) 12 samples per chip
galileo_e1_sinboc_11_gen_int(sinboc_11, _prn, _codeLength); //generate sinboc(1,1) 12 samples per chip
galileo_e1_sinboc_61_gen_int(sinboc_61, _prn, _codeLength); //generate sinboc(6,1) 12 samples per chip
if (_galileo_signal.rfind("1B") != std::string::npos && _galileo_signal.length() >= 2)
{
for (unsigned int i = 0; i < _codeLength; i++)
{
_dest[i] = alpha * static_cast<float>(sinboc_11[i]) +
beta * static_cast<float>(sinboc_61[i]);
_dest[i] = alpha * static_cast<float>(sinboc_11[i]) +
beta * static_cast<float>(sinboc_61[i]);
}
}
else if (_galileo_signal.rfind("1C") != std::string::npos && _galileo_signal.length() >= 2)
{
for (unsigned int i = 0; i < _codeLength; i++)
{
_dest[i] = alpha * static_cast<float>(sinboc_11[i]) -
beta * static_cast<float>(sinboc_61[i]);
_dest[i] = alpha * static_cast<float>(sinboc_11[i]) -
beta * static_cast<float>(sinboc_61[i]);
}
}
else
@@ -143,23 +143,21 @@ void galileo_e1_gen_float(float* _dest, int* _prn, char _Signal[3])
void galileo_e1_code_gen_float_sampled(float* _dest, char _Signal[3],
bool _cboc, unsigned int _prn, signed int _fs, unsigned int _chip_shift,
bool _secondary_flag)
bool _cboc, unsigned int _prn, signed int _fs, unsigned int _chip_shift,
bool _secondary_flag)
{
// This function is based on the GNU software GPS for MATLAB in Kay Borre's book
std::string _galileo_signal = _Signal;
unsigned int _samplesPerCode;
const int _codeFreqBasis = Galileo_E1_CODE_CHIP_RATE_HZ; //Hz
const int _codeFreqBasis = Galileo_E1_CODE_CHIP_RATE_HZ; //Hz
unsigned int _codeLength = Galileo_E1_B_CODE_LENGTH_CHIPS;
int primary_code_E1_chips[static_cast<int>(Galileo_E1_B_CODE_LENGTH_CHIPS)];
_samplesPerCode = static_cast<unsigned int>( static_cast<double>(_fs) / (static_cast<double>(_codeFreqBasis ) / static_cast<double>(_codeLength)));
_samplesPerCode = static_cast<unsigned int>(static_cast<double>(_fs) / (static_cast<double>(_codeFreqBasis) / static_cast<double>(_codeLength)));
const int _samplesPerChip = (_cboc == true) ? 12 : 2;
const unsigned int delay = ((static_cast<int>(Galileo_E1_B_CODE_LENGTH_CHIPS) - _chip_shift)
% static_cast<int>(Galileo_E1_B_CODE_LENGTH_CHIPS))
* _samplesPerCode / Galileo_E1_B_CODE_LENGTH_CHIPS;
const unsigned int delay = ((static_cast<int>(Galileo_E1_B_CODE_LENGTH_CHIPS) - _chip_shift) % static_cast<int>(Galileo_E1_B_CODE_LENGTH_CHIPS)) * _samplesPerCode / Galileo_E1_B_CODE_LENGTH_CHIPS;
galileo_e1_code_gen_int(primary_code_E1_chips, _Signal, _prn); //generate Galileo E1 code, 1 sample per chip
galileo_e1_code_gen_int(primary_code_E1_chips, _Signal, _prn); //generate Galileo E1 code, 1 sample per chip
float* _signal_E1;
@@ -168,16 +166,16 @@ void galileo_e1_code_gen_float_sampled(float* _dest, char _Signal[3],
if (_cboc == true)
{
galileo_e1_gen_float(_signal_E1, primary_code_E1_chips, _Signal); //generate cboc 12 samples per chip
galileo_e1_gen_float(_signal_E1, primary_code_E1_chips, _Signal); //generate cboc 12 samples per chip
}
else
{
int _signal_E1_int[_codeLength];
galileo_e1_sinboc_11_gen_int(_signal_E1_int, primary_code_E1_chips, _codeLength); //generate sinboc(1,1) 2 samples per chip
galileo_e1_sinboc_11_gen_int(_signal_E1_int, primary_code_E1_chips, _codeLength); //generate sinboc(1,1) 2 samples per chip
for( unsigned int ii = 0; ii < _codeLength; ++ii )
for (unsigned int ii = 0; ii < _codeLength; ++ii)
{
_signal_E1[ii] = static_cast< float >( _signal_E1_int[ii] );
_signal_E1[ii] = static_cast<float>(_signal_E1_int[ii]);
}
}
@@ -185,7 +183,7 @@ void galileo_e1_code_gen_float_sampled(float* _dest, char _Signal[3],
{
float* _resampled_signal = new float[_samplesPerCode];
resampler(_signal_E1, _resampled_signal, _samplesPerChip * _codeFreqBasis, _fs,
_codeLength, _samplesPerCode); //resamples code to fs
_codeLength, _samplesPerCode); //resamples code to fs
delete[] _signal_E1;
_signal_E1 = _resampled_signal;
@@ -199,8 +197,7 @@ void galileo_e1_code_gen_float_sampled(float* _dest, char _Signal[3],
{
for (unsigned k = 0; k < _samplesPerCode; k++)
{
_signal_E1C_secondary[i*_samplesPerCode + k] = _signal_E1[k]
* (Galileo_E1_C_SECONDARY_CODE.at(i) == '0' ? 1.0f : -1.0f);
_signal_E1C_secondary[i * _samplesPerCode + k] = _signal_E1[k] * (Galileo_E1_C_SECONDARY_CODE.at(i) == '0' ? 1.0f : -1.0f);
}
}
@@ -220,13 +217,13 @@ void galileo_e1_code_gen_float_sampled(float* _dest, char _Signal[3],
void galileo_e1_code_gen_complex_sampled(std::complex<float>* _dest, char _Signal[3],
bool _cboc, unsigned int _prn, signed int _fs, unsigned int _chip_shift,
bool _secondary_flag)
bool _cboc, unsigned int _prn, signed int _fs, unsigned int _chip_shift,
bool _secondary_flag)
{
std::string _galileo_signal = _Signal;
const int _codeFreqBasis = Galileo_E1_CODE_CHIP_RATE_HZ; //Hz
unsigned int _samplesPerCode = static_cast<unsigned int>( static_cast<double>(_fs) /
(static_cast<double>(_codeFreqBasis ) / static_cast<double>(Galileo_E1_B_CODE_LENGTH_CHIPS)));
const int _codeFreqBasis = Galileo_E1_CODE_CHIP_RATE_HZ; //Hz
unsigned int _samplesPerCode = static_cast<unsigned int>(static_cast<double>(_fs) /
(static_cast<double>(_codeFreqBasis) / static_cast<double>(Galileo_E1_B_CODE_LENGTH_CHIPS)));
if (_galileo_signal.rfind("1C") != std::string::npos && _galileo_signal.length() >= 2 && _secondary_flag)
{
@@ -235,24 +232,24 @@ void galileo_e1_code_gen_complex_sampled(std::complex<float>* _dest, char _Signa
float real_code[_samplesPerCode];
galileo_e1_code_gen_float_sampled( real_code, _Signal, _cboc, _prn, _fs, _chip_shift, _secondary_flag );
galileo_e1_code_gen_float_sampled(real_code, _Signal, _cboc, _prn, _fs, _chip_shift, _secondary_flag);
for( unsigned int ii = 0; ii < _samplesPerCode; ++ii )
for (unsigned int ii = 0; ii < _samplesPerCode; ++ii)
{
_dest[ii] = std::complex< float >( real_code[ii], 0.0f );
_dest[ii] = std::complex<float>(real_code[ii], 0.0f);
}
}
void galileo_e1_code_gen_float_sampled(float* _dest, char _Signal[3],
bool _cboc, unsigned int _prn, signed int _fs, unsigned int _chip_shift)
bool _cboc, unsigned int _prn, signed int _fs, unsigned int _chip_shift)
{
galileo_e1_code_gen_float_sampled(_dest, _Signal, _cboc, _prn, _fs, _chip_shift, false);
}
void galileo_e1_code_gen_complex_sampled(std::complex<float>* _dest, char _Signal[3],
bool _cboc, unsigned int _prn, signed int _fs, unsigned int _chip_shift)
bool _cboc, unsigned int _prn, signed int _fs, unsigned int _chip_shift)
{
galileo_e1_code_gen_complex_sampled(_dest, _Signal, _cboc, _prn, _fs, _chip_shift, false);
}

12
src/algorithms/libs/galileo_e1_signal_processing.h
View File

@@ -41,8 +41,8 @@
*
*/
void galileo_e1_code_gen_float_sampled(float* _dest, char _Signal[3],
bool _cboc, unsigned int _prn, signed int _fs, unsigned int _chip_shift,
bool _secondary_flag);
bool _cboc, unsigned int _prn, signed int _fs, unsigned int _chip_shift,
bool _secondary_flag);
/*!
* \brief This function generates Galileo E1 code (can select E1B or E1C, cboc or sinboc
@@ -50,7 +50,7 @@ void galileo_e1_code_gen_float_sampled(float* _dest, char _Signal[3],
*
*/
void galileo_e1_code_gen_float_sampled(float* _dest, char _Signal[3],
bool _cboc, unsigned int _prn, signed int _fs, unsigned int _chip_shift);
bool _cboc, unsigned int _prn, signed int _fs, unsigned int _chip_shift);
/*!
* \brief This function generates Galileo E1 code (can select E1B or E1C, cboc or sinboc
@@ -58,13 +58,13 @@ void galileo_e1_code_gen_float_sampled(float* _dest, char _Signal[3],
*
*/
void galileo_e1_code_gen_complex_sampled(std::complex<float>* _dest, char _Signal[3],
bool _cboc, unsigned int _prn, signed int _fs, unsigned int _chip_shift,
bool _secondary_flag);
bool _cboc, unsigned int _prn, signed int _fs, unsigned int _chip_shift,
bool _secondary_flag);
/*!
* \brief galileo_e1_code_gen_complex_sampled without _secondary_flag for backward compatibility.
*/
void galileo_e1_code_gen_complex_sampled(std::complex<float>* _dest, char _Signal[3],
bool _cboc, unsigned int _prn, signed int _fs, unsigned int _chip_shift);
bool _cboc, unsigned int _prn, signed int _fs, unsigned int _chip_shift);
#endif /* GNSS_SDR_GALILEO_E1_SIGNAL_PROCESSING_H_ */

21
src/algorithms/libs/galileo_e5_signal_processing.cc
View File

@@ -37,7 +37,6 @@
#include <gnuradio/gr_complex.h>
void galileo_e5_a_code_gen_complex_primary(std::complex<float>* _dest, signed int _prn, char _Signal[3])
{
unsigned int prn = _prn - 1;
@@ -86,10 +85,10 @@ void galileo_e5_a_code_gen_complex_primary(std::complex<float>* _dest, signed in
{
hex_to_binary_converter(a, Galileo_E5a_I_PRIMARY_CODE[prn].at(i));
hex_to_binary_converter(b, Galileo_E5a_Q_PRIMARY_CODE[prn].at(i));
_dest[index] = std::complex<float>(float(a[0]),float(b[0]));
_dest[index + 1] = std::complex<float>(float(a[1]),float(b[1]));
_dest[index + 2] = std::complex<float>(float(a[2]),float(b[2]));
_dest[index + 3] = std::complex<float>(float(a[3]),float(b[3]));
_dest[index] = std::complex<float>(float(a[0]), float(b[0]));
_dest[index + 1] = std::complex<float>(float(a[1]), float(b[1]));
_dest[index + 2] = std::complex<float>(float(a[2]), float(b[2]));
_dest[index + 3] = std::complex<float>(float(a[3]), float(b[3]));
index = index + 4;
}
// last 2 bits are filled up zeros
@@ -101,7 +100,7 @@ void galileo_e5_a_code_gen_complex_primary(std::complex<float>* _dest, signed in
}
void galileo_e5_a_code_gen_complex_sampled(std::complex<float>* _dest, char _Signal[3],
unsigned int _prn, signed int _fs, unsigned int _chip_shift)
unsigned int _prn, signed int _fs, unsigned int _chip_shift)
{
unsigned int _samplesPerCode;
unsigned int delay;
@@ -110,17 +109,19 @@ void galileo_e5_a_code_gen_complex_sampled(std::complex<float>* _dest, char _Sig
std::complex<float>* _code = new std::complex<float>[_codeLength]();
galileo_e5_a_code_gen_complex_primary(_code , _prn , _Signal);
galileo_e5_a_code_gen_complex_primary(_code, _prn, _Signal);
_samplesPerCode = static_cast<unsigned int>(static_cast<double>(_fs) / ( static_cast<double>(_codeFreqBasis) / static_cast<double>(_codeLength)));
_samplesPerCode = static_cast<unsigned int>(static_cast<double>(_fs) / (static_cast<double>(_codeFreqBasis) / static_cast<double>(_codeLength)));
delay = ((_codeLength - _chip_shift) % _codeLength) * _samplesPerCode / _codeLength;
if (_fs != _codeFreqBasis)
{
std::complex<float>* _resampled_signal;
if (posix_memalign((void**)&_resampled_signal, 16, _samplesPerCode * sizeof(gr_complex)) == 0){};
resampler(_code, _resampled_signal, _codeFreqBasis, _fs, _codeLength, _samplesPerCode); //resamples code to fs
if (posix_memalign((void**)&_resampled_signal, 16, _samplesPerCode * sizeof(gr_complex)) == 0)
{
};
resampler(_code, _resampled_signal, _codeFreqBasis, _fs, _codeLength, _samplesPerCode); //resamples code to fs
delete[] _code;
_code = _resampled_signal;
}

4
src/algorithms/libs/galileo_e5_signal_processing.h
View File

@@ -44,14 +44,14 @@
void galileo_e5_a_code_gen_complex_primary(std::complex<float>* _dest, signed int _prn, char _Signal[3]);
void galileo_e5_a_code_gen_tiered(std::complex<float>* _dest,std::complex<float>* _primary ,unsigned int _prn, char _Signal[3]);
void galileo_e5_a_code_gen_tiered(std::complex<float>* _dest, std::complex<float>* _primary, unsigned int _prn, char _Signal[3]);
/*!
* \brief Generates Galileo E5a complex code, shifted to the desired chip and sampled at a frequency fs
* bool _pilot generates E5aQ code if true and E5aI (data signal) if false.
*/
void galileo_e5_a_code_gen_complex_sampled(std::complex<float>* _dest,
char _Signal[3], unsigned int _prn, signed int _fs, unsigned int _chip_shift);
char _Signal[3], unsigned int _prn, signed int _fs, unsigned int _chip_shift);
#endif /* GNSS_SDR_GALILEO_E5_SIGNAL_PROCESSING_H_ */

37
src/algorithms/libs/glonass_l1_signal_processing.cc
View File

@@ -32,9 +32,9 @@
#include "glonass_l1_signal_processing.h"
auto auxCeil = [](float x){ return static_cast<int>(static_cast<long>((x)+1)); };
auto auxCeil = [](float x) { return static_cast<int>(static_cast<long>((x) + 1)); };
void glonass_l1_ca_code_gen_complex(std::complex<float>* _dest,/* signed int _prn,*/ unsigned int _chip_shift)
void glonass_l1_ca_code_gen_complex(std::complex<float>* _dest, /* signed int _prn,*/ unsigned int _chip_shift)
{
const unsigned int _code_length = 511;
bool G1[_code_length];
@@ -44,19 +44,19 @@ void glonass_l1_ca_code_gen_complex(std::complex<float>* _dest,/* signed int _pr
unsigned int delay;
unsigned int lcv, lcv2;
for(lcv = 0; lcv < 9; lcv++)
for (lcv = 0; lcv < 9; lcv++)
{
G1_register[lcv] = 1;
}
/* Generate G1 Register */
for(lcv = 0; lcv < _code_length; lcv++)
for (lcv = 0; lcv < _code_length; lcv++)
{
G1[lcv] = G1_register[2];
feedback1 = G1_register[4]^G1_register[0];
feedback1 = G1_register[4] ^ G1_register[0];
for(lcv2 = 0; lcv2 < 8; lcv2++)
for (lcv2 = 0; lcv2 < 8; lcv2++)
{
G1_register[lcv2] = G1_register[lcv2 + 1];
}
@@ -65,10 +65,10 @@ void glonass_l1_ca_code_gen_complex(std::complex<float>* _dest,/* signed int _pr
}
/* Generate PRN from G1 Register */
for(lcv = 0; lcv < _code_length; lcv++)
for (lcv = 0; lcv < _code_length; lcv++)
{
aux = G1[lcv];
if(aux == true)
if (aux == true)
{
_dest[lcv] = std::complex<float>(1, 0);
}
@@ -84,10 +84,10 @@ void glonass_l1_ca_code_gen_complex(std::complex<float>* _dest,/* signed int _pr
delay %= _code_length;
/* Generate PRN from G1 and G2 Registers */
for(lcv = 0; lcv < _code_length; lcv++)
for (lcv = 0; lcv < _code_length; lcv++)
{
aux = G1[(lcv + _chip_shift) % _code_length];
if(aux == true)
if (aux == true)
{
_dest[lcv] = std::complex<float>(1, 0);
}
@@ -104,7 +104,7 @@ void glonass_l1_ca_code_gen_complex(std::complex<float>* _dest,/* signed int _pr
/*
* Generates complex GLONASS L1 C/A code for the desired SV ID and sampled to specific sampling frequency
*/
void glonass_l1_ca_code_gen_complex_sampled(std::complex<float>* _dest,/* unsigned int _prn,*/ signed int _fs, unsigned int _chip_shift)
void glonass_l1_ca_code_gen_complex_sampled(std::complex<float>* _dest, /* unsigned int _prn,*/ signed int _fs, unsigned int _chip_shift)
{
// This function is based on the GNU software GPS for MATLAB in the Kay Borre book
std::complex<float> _code[511];
@@ -112,16 +112,16 @@ void glonass_l1_ca_code_gen_complex_sampled(std::complex<float>* _dest,/* unsign
float _ts;
float _tc;
float aux;
const signed int _codeFreqBasis = 511000; //Hz
const signed int _codeFreqBasis = 511000; //Hz
const signed int _codeLength = 511;
//--- Find number of samples per spreading code ----------------------------
_samplesPerCode = static_cast<signed int>(static_cast<double>(_fs) / static_cast<double>(_codeFreqBasis / _codeLength));
//--- Find time constants --------------------------------------------------
_ts = 1.0 / static_cast<float>(_fs); // Sampling period in sec
_tc = 1.0 / static_cast<float>(_codeFreqBasis); // C/A chip period in sec
glonass_l1_ca_code_gen_complex(_code, _chip_shift); //generate C/A code 1 sample per chip
_ts = 1.0 / static_cast<float>(_fs); // Sampling period in sec
_tc = 1.0 / static_cast<float>(_codeFreqBasis); // C/A chip period in sec
glonass_l1_ca_code_gen_complex(_code, _chip_shift); //generate C/A code 1 sample per chip
for (signed int i = 0; i < _samplesPerCode; i++)
{
@@ -133,8 +133,8 @@ void glonass_l1_ca_code_gen_complex_sampled(std::complex<float>* _dest,/* unsign
// millisecond).
// _codeValueIndex = ceil((_ts * ((float)i + 1)) / _tc) - 1;
aux = (_ts * (i + 1)) / _tc;
_codeValueIndex = auxCeil( aux ) - 1;
aux = (_ts * (i + 1)) / _tc;
_codeValueIndex = auxCeil(aux) - 1;
//--- Make the digitized version of the C/A code -----------------------
// The "upsampled" code is made by selecting values form the CA code
@@ -143,11 +143,10 @@ void glonass_l1_ca_code_gen_complex_sampled(std::complex<float>* _dest,/* unsign
{
//--- Correct the last index (due to number rounding issues) -----------
_dest[i] = _code[_codeLength - 1];
}
else
{
_dest[i] = _code[_codeValueIndex]; //repeat the chip -> upsample
_dest[i] = _code[_codeValueIndex]; //repeat the chip -> upsample
}
}
}

4
src/algorithms/libs/glonass_l1_signal_processing.h
View File

@@ -39,9 +39,9 @@
void glonass_l1_ca_code_gen_complex(std::complex<float>* _dest, /*signed int _prn,*/ unsigned int _chip_shift);
//! Generates N complex GLONASS L1 C/A codes for the desired SV ID and code shift
void glonass_l1_ca_code_gen_complex_sampled(std::complex<float>* _dest,/* unsigned int _prn,*/ signed int _fs, unsigned int _chip_shift, unsigned int _ncodes);
void glonass_l1_ca_code_gen_complex_sampled(std::complex<float>* _dest, /* unsigned int _prn,*/ signed int _fs, unsigned int _chip_shift, unsigned int _ncodes);
//! Generates complex GLONASS L1 C/A code for the desired SV ID and code shift
void glonass_l1_ca_code_gen_complex_sampled(std::complex<float>* _dest,/* unsigned int _prn,*/ signed int _fs, unsigned int _chip_shift);
void glonass_l1_ca_code_gen_complex_sampled(std::complex<float>* _dest, /* unsigned int _prn,*/ signed int _fs, unsigned int _chip_shift);
#endif /* GNSS_SDR_GLONASS_SDR_SIGNAL_PROCESSING_H_ */

41
src/algorithms/libs/gnss_sdr_flags.cc
View File

@@ -30,7 +30,7 @@
#include "gnss_sdr_flags.h"
#include <boost/filesystem/operations.hpp> // for exists
#include <boost/filesystem/operations.hpp> // for exists
#include <cstdint>
#include <iostream>
#include <string>
@@ -38,13 +38,13 @@
DEFINE_string(c, "-", "Path to the configuration file (if set, overrides --config_file).");
DEFINE_string(config_file, std::string(GNSSSDR_INSTALL_DIR "/share/gnss-sdr/conf/default.conf"),
"Path to the configuration file.");
"Path to the configuration file.");
DEFINE_string(s, "-",
"If defined, path to the file containing the signal samples (overrides the configuration file and --signal_source).");
"If defined, path to the file containing the signal samples (overrides the configuration file and --signal_source).");
DEFINE_string(signal_source, "-",
"If defined, path to the file containing the signal samples (overrides the configuration file).");
"If defined, path to the file containing the signal samples (overrides the configuration file).");
DEFINE_int32(doppler_max, 0, "If defined, sets the maximum Doppler value in the search grid, in Hz (overrides the configuration file).");
@@ -67,36 +67,36 @@ DEFINE_double(pll_bw_hz, 0.0, "If defined, bandwidth of the PLL low pass filter,
#if GFLAGS_GREATER_2_0
static bool ValidateC(const char* flagname, const std::string & value)
static bool ValidateC(const char* flagname, const std::string& value)
{
if (boost::filesystem::exists( value ) or value.compare("-") == 0) // value is ok
if (boost::filesystem::exists(value) or value.compare("-") == 0) // value is ok
return true;
std::cout << "Invalid value for flag -" << flagname << ". The file '" << value << "' does not exist." << std::endl;
std::cout << "GNSS-SDR program ended." << std::endl;
return false;
}
static bool ValidateConfigFile(const char* flagname, const std::string & value)
static bool ValidateConfigFile(const char* flagname, const std::string& value)
{
if (boost::filesystem::exists( value ) or value.compare(std::string(GNSSSDR_INSTALL_DIR "/share/gnss-sdr/conf/default.conf")) == 0) // value is ok
if (boost::filesystem::exists(value) or value.compare(std::string(GNSSSDR_INSTALL_DIR "/share/gnss-sdr/conf/default.conf")) == 0) // value is ok
return true;
std::cout << "Invalid value for flag -" << flagname << ". The file '" << value << "' does not exist." << std::endl;
std::cout << "GNSS-SDR program ended." << std::endl;
return false;
}
static bool ValidateS(const char* flagname, const std::string & value)
static bool ValidateS(const char* flagname, const std::string& value)
{
if (boost::filesystem::exists( value ) or value.compare("-") == 0) // value is ok
if (boost::filesystem::exists(value) or value.compare("-") == 0) // value is ok
return true;
std::cout << "Invalid value for flag -" << flagname << ". The file '" << value << "' does not exist." << std::endl;
std::cout << "GNSS-SDR program ended." << std::endl;
return false;
}
static bool ValidateSignalSource(const char* flagname, const std::string & value)
static bool ValidateSignalSource(const char* flagname, const std::string& value)
{
if (boost::filesystem::exists( value ) or value.compare("-") == 0) // value is ok
if (boost::filesystem::exists(value) or value.compare("-") == 0) // value is ok
return true;
std::cout << "Invalid value for flag -" << flagname << ". The file '" << value << "' does not exist." << std::endl;
std::cout << "GNSS-SDR program ended." << std::endl;
@@ -106,7 +106,7 @@ static bool ValidateSignalSource(const char* flagname, const std::string & value
static bool ValidateDopplerMax(const char* flagname, int32_t value)
{
const int32_t max_value = 1000000;
if (value >= 0 && value < max_value) // value is ok
if (value >= 0 && value < max_value) // value is ok
return true;
std::cout << "Invalid value for flag -" << flagname << ": " << value << ". Allowed range is 0 < " << flagname << " < " << max_value << " Hz." << std::endl;
std::cout << "GNSS-SDR program ended." << std::endl;
@@ -116,7 +116,7 @@ static bool ValidateDopplerMax(const char* flagname, int32_t value)
static bool ValidateDopplerStep(const char* flagname, int32_t value)
{
const int32_t max_value = 10000;
if (value >= 0 && value < max_value) // value is ok
if (value >= 0 && value < max_value) // value is ok
return true;
std::cout << "Invalid value for flag -" << flagname << ": " << value << ". Allowed range is 0 < " << flagname << " < " << max_value << " Hz." << std::endl;
std::cout << "GNSS-SDR program ended." << std::endl;
@@ -126,7 +126,7 @@ static bool ValidateDopplerStep(const char* flagname, int32_t value)
static bool ValidateCn0Samples(const char* flagname, int32_t value)
{
const int32_t max_value = 10000;
if (value > 0 && value < max_value) // value is ok
if (value > 0 && value < max_value) // value is ok
return true;
std::cout << "Invalid value for flag -" << flagname << ": " << value << ". Allowed range is 0 < " << flagname << " < " << max_value << " samples." << std::endl;
std::cout << "GNSS-SDR program ended." << std::endl;
@@ -136,7 +136,7 @@ static bool ValidateCn0Samples(const char* flagname, int32_t value)
static bool ValidateCn0Min(const char* flagname, int32_t value)
{
const int32_t max_value = 100;
if (value > 0 && value < max_value) // value is ok
if (value > 0 && value < max_value) // value is ok
return true;
std::cout << "Invalid value for flag -" << flagname << ": " << value << ". Allowed range is 0 < " << flagname << " < " << max_value << " dB-Hz." << std::endl;
std::cout << "GNSS-SDR program ended." << std::endl;
@@ -146,7 +146,7 @@ static bool ValidateCn0Min(const char* flagname, int32_t value)
static bool ValidateMaxLockFail(const char* flagname, int32_t value)
{
const int32_t max_value = 10000;
if (value > 0 && value < max_value) // value is ok
if (value > 0 && value < max_value) // value is ok
return true;
std::cout << "Invalid value for flag -" << flagname << ": " << value << ". Allowed range is 0 < " << flagname << " < " << max_value << " fails." << std::endl;
std::cout << "GNSS-SDR program ended." << std::endl;
@@ -156,7 +156,7 @@ static bool ValidateMaxLockFail(const char* flagname, int32_t value)
static bool ValidateCarrierLockTh(const char* flagname, double value)
{
const double max_value = 1.508;
if (value > 0.0 && value < max_value) // value is ok
if (value > 0.0 && value < max_value) // value is ok
return true;
std::cout << "Invalid value for flag -" << flagname << ": " << value << ". Allowed range is 0 < " << flagname << " < " << max_value << " rad." << std::endl;
std::cout << "GNSS-SDR program ended." << std::endl;
@@ -166,7 +166,7 @@ static bool ValidateCarrierLockTh(const char* flagname, double value)
static bool ValidateDllBw(const char* flagname, double value)
{
const double max_value = 10000.0;
if (value >= 0.0 && value < max_value) // value is ok
if (value >= 0.0 && value < max_value) // value is ok
return true;
std::cout << "Invalid value for flag -" << flagname << ": " << value << ". Allowed range is 0 < " << flagname << " < " << max_value << " Hz." << std::endl;
std::cout << "GNSS-SDR program ended." << std::endl;
@@ -176,7 +176,7 @@ static bool ValidateDllBw(const char* flagname, double value)
static bool ValidatePllBw(const char* flagname, double value)
{
const double max_value = 10000.0;
if (value >= 0.0 && value < max_value) // value is ok
if (value >= 0.0 && value < max_value) // value is ok
return true;
std::cout << "Invalid value for flag -" << flagname << ": " << value << ". Allowed range is 0 < " << flagname << " < " << max_value << " Hz." << std::endl;
std::cout << "GNSS-SDR program ended." << std::endl;
@@ -198,4 +198,3 @@ DEFINE_validator(pll_bw_hz, &ValidatePllBw);
#endif

28
src/algorithms/libs/gnss_sdr_flags.h
View File

@@ -34,29 +34,29 @@
#include <gflags/gflags.h>
DECLARE_string(c); //<! Path to the configuration file.
DECLARE_string(config_file); //<! Path to the configuration file.
DECLARE_string(c); //<! Path to the configuration file.
DECLARE_string(config_file); //<! Path to the configuration file.
DECLARE_string(log_dir); //<! Path to the folder in which logging will be stored.
DECLARE_string(log_dir); //<! Path to the folder in which logging will be stored.
// Declare flags for signal sources
DECLARE_string(s); //<! Path to the file containing the signal samples.
DECLARE_string(signal_source); //<! Path to the file containing the signal samples.
DECLARE_string(s); //<! Path to the file containing the signal samples.
DECLARE_string(signal_source); //<! Path to the file containing the signal samples.
// Declare flags for acquisition blocks
DECLARE_int32(doppler_max); //<! If defined, maximum Doppler value in the search grid, in Hz (overrides the configuration file).
DECLARE_int32(doppler_step); //<! If defined, sets the frequency step in the search grid, in Hz, in Hz (overrides the configuration file).
DECLARE_int32(doppler_max); //<! If defined, maximum Doppler value in the search grid, in Hz (overrides the configuration file).
DECLARE_int32(doppler_step); //<! If defined, sets the frequency step in the search grid, in Hz, in Hz (overrides the configuration file).
// Declare flags for tracking blocks
DECLARE_int32(cn0_samples); //<! Number of correlator outputs used for CN0 estimation.
DECLARE_int32(cn0_min); //<! Minimum valid CN0 (in dB-Hz).
DECLARE_int32(max_lock_fail); //<! Number number of lock failures before dropping satellite.
DECLARE_double(carrier_lock_th); //<! Carrier lock threshold (in rad).
DECLARE_double(dll_bw_hz); //<! Bandwidth of the DLL low pass filter, in Hz (overrides the configuration file).
DECLARE_double(pll_bw_hz); //<! Bandwidth of the PLL low pass filter, in Hz (overrides the configuration file).
DECLARE_int32(cn0_samples); //<! Number of correlator outputs used for CN0 estimation.
DECLARE_int32(cn0_min); //<! Minimum valid CN0 (in dB-Hz).
DECLARE_int32(max_lock_fail); //<! Number number of lock failures before dropping satellite.
DECLARE_double(carrier_lock_th); //<! Carrier lock threshold (in rad).
DECLARE_double(dll_bw_hz); //<! Bandwidth of the DLL low pass filter, in Hz (overrides the configuration file).
DECLARE_double(pll_bw_hz); //<! Bandwidth of the PLL low pass filter, in Hz (overrides the configuration file).
// Declare flags for PVT
DECLARE_string(RINEX_version); //<! If defined, specifies the RINEX version (2.11 or 3.02). Overrides the configuration file.
DECLARE_string(RINEX_version); //<! If defined, specifies the RINEX version (2.11 or 3.02). Overrides the configuration file.
#endif

22
src/algorithms/libs/gnss_sdr_sample_counter.cc
View File

@@ -33,35 +33,35 @@
#include "gnss_synchro.h"
#include <gnuradio/io_signature.h>
gnss_sdr_sample_counter::gnss_sdr_sample_counter () : gr::sync_block("sample_counter",
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)),
gr::io_signature::make(0,0,0))
gnss_sdr_sample_counter::gnss_sdr_sample_counter() : gr::sync_block("sample_counter",
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)),
gr::io_signature::make(0, 0, 0))
{
this->message_port_register_out(pmt::mp("sample_counter"));
last_T_rx_s = 0;
report_interval_s = 1;//default reporting 1 second
flag_enable_send_msg = false; //enable it for reporting time with asynchronous message
report_interval_s = 1; //default reporting 1 second
flag_enable_send_msg = false; //enable it for reporting time with asynchronous message
}
gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter ()
gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter()
{
gnss_sdr_sample_counter_sptr sample_counter_(new gnss_sdr_sample_counter());
return sample_counter_;
}
int gnss_sdr_sample_counter::work (int noutput_items,
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items __attribute__((unused)))
int gnss_sdr_sample_counter::work(int noutput_items,
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items __attribute__((unused)))
{
const Gnss_Synchro *in = reinterpret_cast<const Gnss_Synchro *>(input_items[0]); // input
const Gnss_Synchro *in = reinterpret_cast<const Gnss_Synchro *>(input_items[0]); // input
double current_T_rx_s = in[noutput_items - 1].Tracking_sample_counter / static_cast<double>(in[noutput_items - 1].fs);
if ((current_T_rx_s - last_T_rx_s) > report_interval_s)
{
std::cout << "Current receiver time: " << floor(current_T_rx_s) << " [s]" << std::endl;
if(flag_enable_send_msg == true)
if (flag_enable_send_msg == true)
{
this->message_port_pub(pmt::mp("receiver_time"), pmt::from_double(current_T_rx_s));
}

8
src/algorithms/libs/gnss_sdr_sample_counter.h
View File

@@ -39,20 +39,20 @@ class gnss_sdr_sample_counter;
typedef boost::shared_ptr<gnss_sdr_sample_counter> gnss_sdr_sample_counter_sptr;
gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter ();
gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter();
class gnss_sdr_sample_counter : public gr::sync_block
{
friend gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter();
gnss_sdr_sample_counter ();
gnss_sdr_sample_counter();
double last_T_rx_s;
double report_interval_s;
bool flag_enable_send_msg;
public:
int work(int noutput_items,
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items);
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items);
};
#endif /*GNSS_SDR_sample_counter_H_*/

40
src/algorithms/libs/gnss_sdr_valve.cc
View File

@@ -34,41 +34,43 @@
#include "control_message_factory.h"
#include <glog/logging.h>
#include <gnuradio/io_signature.h>
#include <algorithm> // for min
#include <cstring> // for memcpy
#include <algorithm> // for min
#include <cstring> // for memcpy
gnss_sdr_valve::gnss_sdr_valve (size_t sizeof_stream_item,
unsigned long long nitems,
gr::msg_queue::sptr queue) : gr::sync_block("valve",
gr::io_signature::make(1, 1, sizeof_stream_item),
gr::io_signature::make(1, 1, sizeof_stream_item) ),
d_nitems(nitems), d_ncopied_items(0), d_queue(queue)
{}
gnss_sdr_valve::gnss_sdr_valve(size_t sizeof_stream_item,
unsigned long long nitems,
gr::msg_queue::sptr queue) : gr::sync_block("valve",
gr::io_signature::make(1, 1, sizeof_stream_item),
gr::io_signature::make(1, 1, sizeof_stream_item)),
d_nitems(nitems),
d_ncopied_items(0),
d_queue(queue)
{
}
boost::shared_ptr<gr::block> gnss_sdr_make_valve (size_t sizeof_stream_item, unsigned long long nitems, gr::msg_queue::sptr queue)
boost::shared_ptr<gr::block> gnss_sdr_make_valve(size_t sizeof_stream_item, unsigned long long nitems, gr::msg_queue::sptr queue)
{
boost::shared_ptr<gnss_sdr_valve> valve_(new gnss_sdr_valve(sizeof_stream_item, nitems, queue));
return valve_;
}
int gnss_sdr_valve::work (int noutput_items,
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items)
int gnss_sdr_valve::work(int noutput_items,
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items)
{
if (d_ncopied_items >= d_nitems)
{
ControlMessageFactory* cmf = new ControlMessageFactory();
d_queue->handle(cmf->GetQueueMessage(200,0));
LOG(INFO) << "Stopping receiver, "<< d_ncopied_items << " samples processed";
ControlMessageFactory *cmf = new ControlMessageFactory();
d_queue->handle(cmf->GetQueueMessage(200, 0));
LOG(INFO) << "Stopping receiver, " << d_ncopied_items << " samples processed";
delete cmf;
return -1; // Done!
return -1; // Done!
}
unsigned long long n = std::min(d_nitems - d_ncopied_items, static_cast<long long unsigned int>(noutput_items));
if (n == 0) return 0;
memcpy (output_items[0], input_items[0], n * input_signature()->sizeof_stream_item(0));
memcpy(output_items[0], input_items[0], n * input_signature()->sizeof_stream_item(0));
//for(long long i = 0; i++; i<n)
// {
// output_items[i] = input_items[i];

20
src/algorithms/libs/gnss_sdr_valve.h
View File

@@ -38,9 +38,9 @@
#include <boost/shared_ptr.hpp>
boost::shared_ptr<gr::block> gnss_sdr_make_valve (size_t sizeof_stream_item,
unsigned long long nitems,
gr::msg_queue::sptr queue);
boost::shared_ptr<gr::block> gnss_sdr_make_valve(size_t sizeof_stream_item,
unsigned long long nitems,
gr::msg_queue::sptr queue);
/*!
* \brief Implementation of a GNU Radio block that sends a STOP message to the
* control queue right after a specific number of samples have passed through it.
@@ -48,19 +48,19 @@ boost::shared_ptr<gr::block> gnss_sdr_make_valve (size_t sizeof_stream_item,
class gnss_sdr_valve : public gr::sync_block
{
friend boost::shared_ptr<gr::block> gnss_sdr_make_valve(size_t sizeof_stream_item,
unsigned long long nitems,
gr::msg_queue::sptr queue);
gnss_sdr_valve (size_t sizeof_stream_item,
unsigned long long nitems,
gr::msg_queue::sptr queue);
unsigned long long nitems,
gr::msg_queue::sptr queue);
gnss_sdr_valve(size_t sizeof_stream_item,
unsigned long long nitems,
gr::msg_queue::sptr queue);
unsigned long long d_nitems;
unsigned long long d_ncopied_items;
gr::msg_queue::sptr d_queue;
public:
int work(int noutput_items,
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items);
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items);
};
#endif /*GNSS_SDR_GNSS_SDR_VALVE_H_*/

218
src/algorithms/libs/gnss_signal_processing.cc
View File

@@ -36,13 +36,13 @@
#include <gnuradio/fxpt_nco.h>
auto auxCeil2 = [](float x){ return static_cast<int>(static_cast<long>((x)+1)); };
auto auxCeil2 = [](float x) { return static_cast<int>(static_cast<long>((x) + 1)); };
void complex_exp_gen(std::complex<float>* _dest, double _f, double _fs, unsigned int _samps)
{
gr::fxpt_nco d_nco;
d_nco.set_freq((GPS_TWO_PI * _f) / _fs);
d_nco.sincos(_dest, _samps, 1);
d_nco.sincos(_dest, _samps, 1);
}
@@ -50,120 +50,120 @@ void complex_exp_gen_conj(std::complex<float>* _dest, double _f, double _fs, uns
{
gr::fxpt_nco d_nco;
d_nco.set_freq(-(GPS_TWO_PI * _f) / _fs);
d_nco.sincos(_dest, _samps, 1);
d_nco.sincos(_dest, _samps, 1);
}
void hex_to_binary_converter(int * _dest, char _from)
void hex_to_binary_converter(int* _dest, char _from)
{
switch(_from)
{
case '0':
*(_dest) = 1;
*(_dest+1) = 1;
*(_dest+2) = 1;
*(_dest+3) = 1;
break;
case '1':
*(_dest) = 1;
*(_dest+1) = 1;
*(_dest+2) = 1;
*(_dest+3) = -1;
break;
case '2':
*(_dest) = 1;
*(_dest+1) = 1;
*(_dest+2) = -1;
*(_dest+3) = 1;
break;
case '3':
*(_dest) = 1;
*(_dest+1) = 1;
*(_dest+2) = -1;
*(_dest+3) = -1;
break;
case '4':
*(_dest) = 1;
*(_dest+1) = -1;
*(_dest+2) = 1;
*(_dest+3) = 1;
break;
case '5':
*(_dest) = 1;
*(_dest+1) = -1;
*(_dest+2) = 1;
*(_dest+3) = -1;
break;
case '6':
*(_dest) = 1;
*(_dest+1) = -1;
*(_dest+2) = -1;
*(_dest+3) = 1;
break;
case '7':
*(_dest) = 1;
*(_dest+1) = -1;
*(_dest+2) = -1;
*(_dest+3) = -1;
break;
case '8':
*(_dest) = -1;
*(_dest+1) = 1;
*(_dest+2) = 1;
*(_dest+3) = 1;
break;
case '9':
*(_dest) = -1;
*(_dest+1) = 1;
*(_dest+2) = 1;
*(_dest+3) = -1;
break;
case 'A':
*(_dest) = -1;
*(_dest+1) = 1;
*(_dest+2) = -1;
*(_dest+3) = 1;
break;
case 'B':
*(_dest) = -1;
*(_dest+1) = 1;
*(_dest+2) = -1;
*(_dest+3) = -1;
break;
case 'C':
*(_dest) = -1;
*(_dest+1) = -1;
*(_dest+2) = 1;
*(_dest+3) = 1;
break;
case 'D':
*(_dest) = -1;
*(_dest+1) = -1;
*(_dest+2) = 1;
*(_dest+3) = -1;
break;
case 'E':
*(_dest) = -1;
*(_dest+1) = -1;
*(_dest+2) = -1;
*(_dest+3) = 1;
break;
case 'F':
*(_dest) = -1;
*(_dest+1) = -1;
*(_dest+2) = -1;
*(_dest+3) = -1;
break;
}
switch (_from)
{
case '0':
*(_dest) = 1;
*(_dest + 1) = 1;
*(_dest + 2) = 1;
*(_dest + 3) = 1;
break;
case '1':
*(_dest) = 1;
*(_dest + 1) = 1;
*(_dest + 2) = 1;
*(_dest + 3) = -1;
break;
case '2':
*(_dest) = 1;
*(_dest + 1) = 1;
*(_dest + 2) = -1;
*(_dest + 3) = 1;
break;
case '3':
*(_dest) = 1;
*(_dest + 1) = 1;
*(_dest + 2) = -1;
*(_dest + 3) = -1;
break;
case '4':
*(_dest) = 1;
*(_dest + 1) = -1;
*(_dest + 2) = 1;
*(_dest + 3) = 1;
break;
case '5':
*(_dest) = 1;
*(_dest + 1) = -1;
*(_dest + 2) = 1;
*(_dest + 3) = -1;
break;
case '6':
*(_dest) = 1;
*(_dest + 1) = -1;
*(_dest + 2) = -1;
*(_dest + 3) = 1;
break;
case '7':
*(_dest) = 1;
*(_dest + 1) = -1;
*(_dest + 2) = -1;
*(_dest + 3) = -1;
break;
case '8':
*(_dest) = -1;
*(_dest + 1) = 1;
*(_dest + 2) = 1;
*(_dest + 3) = 1;
break;
case '9':
*(_dest) = -1;
*(_dest + 1) = 1;
*(_dest + 2) = 1;
*(_dest + 3) = -1;
break;
case 'A':
*(_dest) = -1;
*(_dest + 1) = 1;
*(_dest + 2) = -1;
*(_dest + 3) = 1;
break;
case 'B':
*(_dest) = -1;
*(_dest + 1) = 1;
*(_dest + 2) = -1;
*(_dest + 3) = -1;
break;
case 'C':
*(_dest) = -1;
*(_dest + 1) = -1;
*(_dest + 2) = 1;
*(_dest + 3) = 1;
break;
case 'D':
*(_dest) = -1;
*(_dest + 1) = -1;
*(_dest + 2) = 1;
*(_dest + 3) = -1;
break;
case 'E':
*(_dest) = -1;
*(_dest + 1) = -1;
*(_dest + 2) = -1;
*(_dest + 3) = 1;
break;
case 'F':
*(_dest) = -1;
*(_dest + 1) = -1;
*(_dest + 2) = -1;
*(_dest + 3) = -1;
break;
}
}
void resampler(float* _from, float* _dest, float _fs_in,
float _fs_out, unsigned int _length_in, unsigned int _length_out)
float _fs_out, unsigned int _length_in, unsigned int _length_out)
{
unsigned int _codeValueIndex;
float aux;
//--- Find time constants --------------------------------------------------
const float _t_in = 1 / _fs_in; // Incoming sampling period in sec
const float _t_out = 1 / _fs_out; // Out sampling period in sec
const float _t_in = 1 / _fs_in; // Incoming sampling period in sec
const float _t_out = 1 / _fs_out; // Out sampling period in sec
for (unsigned int i = 0; i < _length_out - 1; i++)
{
//=== Digitizing =======================================================
@@ -180,13 +180,13 @@ void resampler(float* _from, float* _dest, float _fs_in,
}
void resampler(std::complex<float>* _from, std::complex<float>* _dest, float _fs_in,
float _fs_out, unsigned int _length_in, unsigned int _length_out)
float _fs_out, unsigned int _length_in, unsigned int _length_out)
{
unsigned int _codeValueIndex;
float aux;
//--- Find time constants --------------------------------------------------
const float _t_in = 1 / _fs_in; // Incoming sampling period in sec
const float _t_out = 1 / _fs_out; // Out sampling period in sec
const float _t_in = 1 / _fs_in; // Incoming sampling period in sec
const float _t_out = 1 / _fs_out; // Out sampling period in sec
for (unsigned int i = 0; i < _length_out - 1; i++)
{
//=== Digitizing =======================================================

14
src/algorithms/libs/gnss_signal_processing.h
View File

@@ -43,14 +43,14 @@
*
*/
void complex_exp_gen(std::complex<float>* _dest, double _f, double _fs,
unsigned int _samps);
unsigned int _samps);
/*!
* \brief This function generates a conjugate complex exponential in _dest.
*
*/
void complex_exp_gen_conj(std::complex<float>* _dest, double _f, double _fs,
unsigned int _samps);
unsigned int _samps);
/*!
@@ -58,21 +58,21 @@ void complex_exp_gen_conj(std::complex<float>* _dest, double _f, double _fs,
* to binary (the output are 4 ints with +1 or -1 values).
*
*/
void hex_to_binary_converter(int * _dest, char _from);
void hex_to_binary_converter(int* _dest, char _from);
/*!
* \brief This function resamples a sequence of float values.
*
*/
void resampler(float* _from, float* _dest,
float _fs_in, float _fs_out, unsigned int _length_in,
unsigned int _length_out);
float _fs_in, float _fs_out, unsigned int _length_in,
unsigned int _length_out);
/*!
* \brief This function resamples a sequence of complex values.
*
*/
void resampler(std::complex<float>* _from, std::complex<float>* _dest,
float _fs_in, float _fs_out, unsigned int _length_in,
unsigned int _length_out);
float _fs_in, float _fs_out, unsigned int _length_in,
unsigned int _length_out);
#endif /* GNSS_SDR_GNSS_SIGNAL_PROCESSING_H_ */

16
src/algorithms/libs/gps_l2c_signal.cc
View File

@@ -38,17 +38,17 @@
int32_t gps_l2c_m_shift(int32_t x)
{
return static_cast<int32_t>((x >> 1)^((x & 1) * 0445112474));
return static_cast<int32_t>((x >> 1) ^ ((x & 1) * 0445112474));
}
void gps_l2c_m_code(int32_t * _dest, unsigned int _prn)
void gps_l2c_m_code(int32_t* _dest, unsigned int _prn)
{
int32_t x;
x = GPS_L2C_M_INIT_REG[ _prn - 1];
x = GPS_L2C_M_INIT_REG[_prn - 1];
for (int n = 0; n < GPS_L2_M_CODE_LENGTH_CHIPS; n++)
{
_dest[n] = static_cast<int8_t>(x&1);
_dest[n] = static_cast<int8_t>(x & 1);
x = gps_l2c_m_shift(x);
}
}
@@ -92,7 +92,7 @@ void gps_l2c_m_code_gen_complex_sampled(std::complex<float>* _dest, unsigned int
_samplesPerCode = static_cast<int>(static_cast<double>(_fs) / (static_cast<double>(GPS_L2_M_CODE_RATE_HZ) / static_cast<double>(_codeLength)));
//--- Find time constants --------------------------------------------------
_ts = 1.0 / static_cast<float>(_fs); // Sampling period in sec
_ts = 1.0 / static_cast<float>(_fs); // Sampling period in sec
_tc = 1.0 / static_cast<float>(GPS_L2_M_CODE_RATE_HZ); // C/A chip period in sec
//float aux;
@@ -114,12 +114,8 @@ void gps_l2c_m_code_gen_complex_sampled(std::complex<float>* _dest, unsigned int
}
else
{
_dest[i] = std::complex<float>(1.0 - 2.0 * _code[_codeValueIndex], 0); //repeat the chip -> upsample
_dest[i] = std::complex<float>(1.0 - 2.0 * _code[_codeValueIndex], 0); //repeat the chip -> upsample
}
}
delete[] _code;
}

42
src/algorithms/libs/gps_l5_signal.cc
View File

@@ -40,9 +40,9 @@
std::deque<bool> l5i_xa_shift(std::deque<bool> xa)
{
if (xa == std::deque<bool>{1,1,1,1,1,1,1,1,1,1,1,0,1})
if (xa == std::deque<bool>{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1})
{
return std::deque<bool>{1,1,1,1,1,1,1,1,1,1,1,1,1};
return std::deque<bool>{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1};
}
else
{
@@ -55,9 +55,9 @@ std::deque<bool> l5i_xa_shift(std::deque<bool> xa)
std::deque<bool> l5q_xa_shift(std::deque<bool> xa)
{
if (xa == std::deque<bool>{1,1,1,1,1,1,1,1,1,1,1,0,1})
if (xa == std::deque<bool>{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1})
{
return std::deque<bool>{1,1,1,1,1,1,1,1,1,1,1,1,1};
return std::deque<bool>{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1};
}
else
{
@@ -78,7 +78,7 @@ std::deque<bool> l5i_xb_shift(std::deque<bool> xb)
std::deque<bool> l5q_xb_shift(std::deque<bool> xb)
{
std::deque<bool> out(xb.begin(), xb.end()-1);
std::deque<bool> out(xb.begin(), xb.end() - 1);
out.push_front(xb[12] xor xb[11] xor xb[7] xor xb[6] xor xb[5] xor xb[3] xor xb[2] xor xb[0]);
return out;
}
@@ -86,7 +86,7 @@ std::deque<bool> l5q_xb_shift(std::deque<bool> xb)
std::deque<bool> make_l5i_xa()
{
std::deque<bool> xa = {1,1,1,1,1,1,1,1,1,1,1,1,1};
std::deque<bool> xa = {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1};
std::deque<bool> y(GPS_L5i_CODE_LENGTH_CHIPS, 0);
for (int i = 0; i < GPS_L5i_CODE_LENGTH_CHIPS; i++)
@@ -100,8 +100,8 @@ std::deque<bool> make_l5i_xa()
std::deque<bool> make_l5i_xb()
{
std::deque<bool> xb = {1,1,1,1,1,1,1,1,1,1,1,1,1};
std::deque<bool> y(GPS_L5i_CODE_LENGTH_CHIPS,0);
std::deque<bool> xb = {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1};
std::deque<bool> y(GPS_L5i_CODE_LENGTH_CHIPS, 0);
for (int i = 0; i < GPS_L5i_CODE_LENGTH_CHIPS; i++)
{
@@ -114,7 +114,7 @@ std::deque<bool> make_l5i_xb()
std::deque<bool> make_l5q_xa()
{
std::deque<bool> xa = {1,1,1,1,1,1,1,1,1,1,1,1,1};
std::deque<bool> xa = {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1};
std::deque<bool> y(GPS_L5q_CODE_LENGTH_CHIPS, 0);
for (int i = 0; i < GPS_L5q_CODE_LENGTH_CHIPS; i++)
@@ -128,7 +128,7 @@ std::deque<bool> make_l5q_xa()
std::deque<bool> make_l5q_xb()
{
std::deque<bool> xb = {1,1,1,1,1,1,1,1,1,1,1,1,1};
std::deque<bool> xb = {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1};
std::deque<bool> y(GPS_L5q_CODE_LENGTH_CHIPS, 0);
for (int i = 0; i < GPS_L5q_CODE_LENGTH_CHIPS; i++)
@@ -140,7 +140,7 @@ std::deque<bool> make_l5q_xb()
}
void make_l5i(int32_t * _dest, int prn)
void make_l5i(int32_t* _dest, int prn)
{
int xb_offset = GPS_L5i_INIT_REG[prn];
@@ -160,7 +160,7 @@ void make_l5i(int32_t * _dest, int prn)
}
void make_l5q(int32_t * _dest, int prn)
void make_l5q(int32_t* _dest, int prn)
{
int xb_offset = GPS_L5q_INIT_REG[prn];
@@ -186,7 +186,7 @@ void gps_l5i_code_gen_complex(std::complex<float>* _dest, unsigned int _prn)
if (_prn > 0 and _prn < 51)
{
make_l5i(_code, _prn - 1);
make_l5i(_code, _prn - 1);
}
for (signed int i = 0; i < GPS_L5i_CODE_LENGTH_CHIPS; i++)
@@ -206,7 +206,7 @@ void gps_l5i_code_gen_complex_sampled(std::complex<float>* _dest, unsigned int _
int32_t* _code = new int32_t[GPS_L5i_CODE_LENGTH_CHIPS];
if (_prn > 0 and _prn < 51)
{
make_l5i(_code, _prn - 1);
make_l5i(_code, _prn - 1);
}
signed int _samplesPerCode, _codeValueIndex;
@@ -218,7 +218,7 @@ void gps_l5i_code_gen_complex_sampled(std::complex<float>* _dest, unsigned int _
_samplesPerCode = static_cast<int>(static_cast<double>(_fs) / (static_cast<double>(GPS_L5i_CODE_RATE_HZ) / static_cast<double>(_codeLength)));
//--- Find time constants --------------------------------------------------
_ts = 1.0 / static_cast<float>(_fs); // Sampling period in sec
_ts = 1.0 / static_cast<float>(_fs); // Sampling period in sec
_tc = 1.0 / static_cast<float>(GPS_L5i_CODE_RATE_HZ); // C/A chip period in sec
//float aux;
@@ -240,7 +240,7 @@ void gps_l5i_code_gen_complex_sampled(std::complex<float>* _dest, unsigned int _
}
else
{
_dest[i] = std::complex<float>(1.0 - 2.0 * _code[_codeValueIndex], 0); //repeat the chip -> upsample
_dest[i] = std::complex<float>(1.0 - 2.0 * _code[_codeValueIndex], 0); //repeat the chip -> upsample
}
}
delete[] _code;
@@ -253,7 +253,7 @@ void gps_l5q_code_gen_complex(std::complex<float>* _dest, unsigned int _prn)
if (_prn > 0 and _prn < 51)
{
make_l5q(_code, _prn - 1);
make_l5q(_code, _prn - 1);
}
for (signed int i = 0; i < GPS_L5q_CODE_LENGTH_CHIPS; i++)
@@ -273,7 +273,7 @@ void gps_l5q_code_gen_complex_sampled(std::complex<float>* _dest, unsigned int _
int32_t* _code = new int32_t[GPS_L5q_CODE_LENGTH_CHIPS];
if (_prn > 0 and _prn < 51)
{
make_l5q(_code, _prn - 1);
make_l5q(_code, _prn - 1);
}
signed int _samplesPerCode, _codeValueIndex;
@@ -285,7 +285,7 @@ void gps_l5q_code_gen_complex_sampled(std::complex<float>* _dest, unsigned int _
_samplesPerCode = static_cast<int>(static_cast<double>(_fs) / (static_cast<double>(GPS_L5q_CODE_RATE_HZ) / static_cast<double>(_codeLength)));
//--- Find time constants --------------------------------------------------
_ts = 1.0 / static_cast<float>(_fs); // Sampling period in sec
_ts = 1.0 / static_cast<float>(_fs); // Sampling period in sec
_tc = 1.0 / static_cast<float>(GPS_L5q_CODE_RATE_HZ); // C/A chip period in sec
//float aux;
@@ -307,10 +307,8 @@ void gps_l5q_code_gen_complex_sampled(std::complex<float>* _dest, unsigned int _
}
else
{
_dest[i] = std::complex<float>(1.0 - 2.0 * _code[_codeValueIndex], 0); //repeat the chip -> upsample
_dest[i] = std::complex<float>(1.0 - 2.0 * _code[_codeValueIndex], 0); //repeat the chip -> upsample
}
}
delete[] _code;
}

62
src/algorithms/libs/gps_sdr_signal_processing.cc
View File

@@ -32,7 +32,7 @@
#include "gps_sdr_signal_processing.h"
auto auxCeil = [](float x){ return static_cast<int>(static_cast<long>((x)+1)); };
auto auxCeil = [](float x) { return static_cast<int>(static_cast<long>((x) + 1)); };
void gps_l1_ca_code_gen_int(int* _dest, signed int _prn, unsigned int _chip_shift)
{
@@ -47,15 +47,15 @@ void gps_l1_ca_code_gen_int(int* _dest, signed int _prn, unsigned int _chip_shif
signed int prn_idx;
/* G2 Delays as defined in GPS-ISD-200D */
const signed int delays[51] = {5 /*PRN1*/, 6, 7, 8, 17, 18, 139, 140, 141, 251, 252, 254 ,255, 256, 257, 258, 469, 470, 471, 472,
473, 474, 509, 512, 513, 514, 515, 516, 859, 860, 861, 862 /*PRN32*/,
145 /*PRN120*/, 175, 52, 21, 237, 235, 886, 657, 634, 762,
355, 1012, 176, 603, 130, 359, 595, 68, 386 /*PRN138*/};
const signed int delays[51] = {5 /*PRN1*/, 6, 7, 8, 17, 18, 139, 140, 141, 251, 252, 254, 255, 256, 257, 258, 469, 470, 471, 472,
473, 474, 509, 512, 513, 514, 515, 516, 859, 860, 861, 862 /*PRN32*/,
145 /*PRN120*/, 175, 52, 21, 237, 235, 886, 657, 634, 762,
355, 1012, 176, 603, 130, 359, 595, 68, 386 /*PRN138*/};
// compute delay array index for given PRN number
if(120 <= _prn && _prn <= 138)
if (120 <= _prn && _prn <= 138)
{
prn_idx = _prn - 88; // SBAS PRNs are at array indices 31 to 50 (offset: -120+33-1 =-88)
prn_idx = _prn - 88; // SBAS PRNs are at array indices 31 to 50 (offset: -120+33-1 =-88)
}
else
{
@@ -63,25 +63,25 @@ void gps_l1_ca_code_gen_int(int* _dest, signed int _prn, unsigned int _chip_shif
}
/* A simple error check */
if((prn_idx < 0) || (prn_idx > 51))
if ((prn_idx < 0) || (prn_idx > 51))
return;
for(lcv = 0; lcv < 10; lcv++)
for (lcv = 0; lcv < 10; lcv++)
{
G1_register[lcv] = 1;
G2_register[lcv] = 1;
}
/* Generate G1 & G2 Register */
for(lcv = 0; lcv < _code_length; lcv++)
for (lcv = 0; lcv < _code_length; lcv++)
{
G1[lcv] = G1_register[0];
G2[lcv] = G2_register[0];
feedback1 = G1_register[7]^G1_register[0];
feedback1 = G1_register[7] ^ G1_register[0];
feedback2 = (G2_register[8] + G2_register[7] + G2_register[4] + G2_register[2] + G2_register[1] + G2_register[0]) & 0x1;
for(lcv2 = 0; lcv2 < 9; lcv2++)
for (lcv2 = 0; lcv2 < 9; lcv2++)
{
G1_register[lcv2] = G1_register[lcv2 + 1];
G2_register[lcv2] = G2_register[lcv2 + 1];
@@ -97,10 +97,10 @@ void gps_l1_ca_code_gen_int(int* _dest, signed int _prn, unsigned int _chip_shif
delay %= _code_length;
/* Generate PRN from G1 and G2 Registers */
for(lcv = 0; lcv < _code_length; lcv++)
for (lcv = 0; lcv < _code_length; lcv++)
{
aux = G1[(lcv + _chip_shift) % _code_length]^G2[delay];
if(aux == true)
aux = G1[(lcv + _chip_shift) % _code_length] ^ G2[delay];
if (aux == true)
{
_dest[lcv] = 1;
}
@@ -117,13 +117,13 @@ void gps_l1_ca_code_gen_int(int* _dest, signed int _prn, unsigned int _chip_shif
void gps_l1_ca_code_gen_float(float* _dest, signed int _prn, unsigned int _chip_shift)
{
unsigned int _code_length = 1023;
int ca_code_int[ _code_length ];
int ca_code_int[_code_length];
gps_l1_ca_code_gen_int( ca_code_int, _prn, _chip_shift );
gps_l1_ca_code_gen_int(ca_code_int, _prn, _chip_shift);
for( unsigned int ii = 0; ii < _code_length; ++ii )
for (unsigned int ii = 0; ii < _code_length; ++ii)
{
_dest[ii] = static_cast<float>( ca_code_int[ii] );
_dest[ii] = static_cast<float>(ca_code_int[ii]);
}
}
@@ -131,13 +131,13 @@ void gps_l1_ca_code_gen_float(float* _dest, signed int _prn, unsigned int _chip_
void gps_l1_ca_code_gen_complex(std::complex<float>* _dest, signed int _prn, unsigned int _chip_shift)
{
unsigned int _code_length = 1023;
int ca_code_int[ _code_length ];
int ca_code_int[_code_length];
gps_l1_ca_code_gen_int( ca_code_int, _prn, _chip_shift );
gps_l1_ca_code_gen_int(ca_code_int, _prn, _chip_shift);
for( unsigned int ii = 0; ii < _code_length; ++ii )
for (unsigned int ii = 0; ii < _code_length; ++ii)
{
_dest[ii] = std::complex<float>( static_cast<float>(ca_code_int[ii]), 0.0f );
_dest[ii] = std::complex<float>(static_cast<float>(ca_code_int[ii]), 0.0f);
}
}
@@ -153,16 +153,16 @@ void gps_l1_ca_code_gen_complex_sampled(std::complex<float>* _dest, unsigned int
float _ts;
float _tc;
float aux;
const signed int _codeFreqBasis = 1023000; //Hz
const signed int _codeFreqBasis = 1023000; //Hz
const signed int _codeLength = 1023;
//--- Find number of samples per spreading code ----------------------------
_samplesPerCode = static_cast<signed int>(static_cast<double>(_fs) / static_cast<double>(_codeFreqBasis / _codeLength));
//--- Find time constants --------------------------------------------------
_ts = 1.0 / static_cast<float>(_fs); // Sampling period in sec
_tc = 1.0 / static_cast<float>(_codeFreqBasis); // C/A chip period in sec
gps_l1_ca_code_gen_complex(_code, _prn, _chip_shift); //generate C/A code 1 sample per chip
_ts = 1.0 / static_cast<float>(_fs); // Sampling period in sec
_tc = 1.0 / static_cast<float>(_codeFreqBasis); // C/A chip period in sec
gps_l1_ca_code_gen_complex(_code, _prn, _chip_shift); //generate C/A code 1 sample per chip
for (signed int i = 0; i < _samplesPerCode; i++)
{
@@ -174,8 +174,8 @@ void gps_l1_ca_code_gen_complex_sampled(std::complex<float>* _dest, unsigned int
// millisecond).
// _codeValueIndex = ceil((_ts * ((float)i + 1)) / _tc) - 1;
aux = (_ts * (i + 1)) / _tc;
_codeValueIndex = auxCeil( aux ) - 1;
aux = (_ts * (i + 1)) / _tc;
_codeValueIndex = auxCeil(aux) - 1;
//--- Make the digitized version of the C/A code -----------------------
// The "upsampled" code is made by selecting values form the CA code
@@ -184,12 +184,10 @@ void gps_l1_ca_code_gen_complex_sampled(std::complex<float>* _dest, unsigned int
{
//--- Correct the last index (due to number rounding issues) -----------
_dest[i] = _code[_codeLength - 1];
}
else
{
_dest[i] = _code[_codeValueIndex]; //repeat the chip -> upsample
_dest[i] = _code[_codeValueIndex]; //repeat the chip -> upsample
}
}
}

4
src/algorithms/libs/gps_sdr_signal_processing.h
View File

@@ -35,10 +35,10 @@
#include <complex>
//!Generates int GPS L1 C/A code for the desired SV ID and code shift
//!Generates int GPS L1 C/A code for the desired SV ID and code shift
void gps_l1_ca_code_gen_int(int* _dest, signed int _prn, unsigned int _chip_shift);
//!Generates float GPS L1 C/A code for the desired SV ID and code shift
//!Generates float GPS L1 C/A code for the desired SV ID and code shift
void gps_l1_ca_code_gen_float(float* _dest, signed int _prn, unsigned int _chip_shift);
//!Generates complex GPS L1 C/A code for the desired SV ID and code shift, and sampled to specific sampling frequency

65
src/algorithms/libs/pass_through.cc
View File

@@ -39,16 +39,15 @@
using google::LogMessage;
Pass_Through::Pass_Through(ConfigurationInterface* configuration, std::string role,
unsigned int in_streams,
unsigned int out_streams) :
role_(role),
in_streams_(in_streams),
out_streams_(out_streams)
unsigned int in_streams,
unsigned int out_streams) : role_(role),
in_streams_(in_streams),
out_streams_(out_streams)
{
std::string default_item_type = "gr_complex";
std::string input_type = configuration->property(role + ".input_item_type", default_item_type);
std::string output_type = configuration->property(role + ".output_item_type", default_item_type);
if(input_type.compare(output_type) != 0)
if (input_type.compare(output_type) != 0)
{
LOG(WARNING) << "input_item_type and output_item_type are different in a Pass_Through implementation! Taking "
<< input_type
@@ -58,46 +57,46 @@ Pass_Through::Pass_Through(ConfigurationInterface* configuration, std::string ro
item_type_ = configuration->property(role + ".item_type", input_type);
inverted_spectrum = configuration->property(role + ".inverted_spectrum", false);
if(item_type_.compare("float") == 0)
if (item_type_.compare("float") == 0)
{
item_size_ = sizeof(float);
}
else if(item_type_.compare("gr_complex") == 0)
else if (item_type_.compare("gr_complex") == 0)
{
item_size_ = sizeof(gr_complex);
if(inverted_spectrum)
if (inverted_spectrum)
{
conjugate_cc_ = make_conjugate_cc();
}
}
else if(item_type_.compare("short") == 0)
else if (item_type_.compare("short") == 0)
{
item_size_ = sizeof(int16_t);
}
else if(item_type_.compare("ishort") == 0)
else if (item_type_.compare("ishort") == 0)
{
item_size_ = sizeof(int16_t);
}
else if(item_type_.compare("cshort") == 0)
else if (item_type_.compare("cshort") == 0)
{
item_size_ = sizeof(lv_16sc_t);
if(inverted_spectrum)
if (inverted_spectrum)
{
conjugate_sc_ = make_conjugate_sc();
}
}
else if(item_type_.compare("byte") == 0)
else if (item_type_.compare("byte") == 0)
{
item_size_ = sizeof(int8_t);
}
else if(item_type_.compare("ibyte") == 0)
else if (item_type_.compare("ibyte") == 0)
{
item_size_ = sizeof(int8_t);
}
else if(item_type_.compare("cbyte") == 0)
else if (item_type_.compare("cbyte") == 0)
{
item_size_ = sizeof(lv_8sc_t);
if(inverted_spectrum)
if (inverted_spectrum)
{
conjugate_ic_ = make_conjugate_ic();
}
@@ -113,41 +112,42 @@ Pass_Through::Pass_Through(ConfigurationInterface* configuration, std::string ro
}
Pass_Through::~Pass_Through()
{}
{
}
void Pass_Through::connect(gr::top_block_sptr top_block)
{
if(top_block) { /* top_block is not null */};
if (top_block)
{ /* top_block is not null */
};
DLOG(INFO) << "nothing to connect internally";
}
void Pass_Through::disconnect(gr::top_block_sptr top_block)
{
if(top_block) { /* top_block is not null */};
if (top_block)
{ /* top_block is not null */
};
// Nothing to disconnect
}
gr::basic_block_sptr Pass_Through::get_left_block()
{
if(inverted_spectrum)
if (inverted_spectrum)
{
if(item_type_.compare("gr_complex") == 0)
if (item_type_.compare("gr_complex") == 0)
{
return conjugate_cc_;
}
else if(item_type_.compare("cshort") == 0)
else if (item_type_.compare("cshort") == 0)
{
return conjugate_sc_;
}
else if(item_type_.compare("cbyte") == 0)
else if (item_type_.compare("cbyte") == 0)
{
return conjugate_ic_;
}
@@ -162,20 +162,19 @@ gr::basic_block_sptr Pass_Through::get_left_block()
}
gr::basic_block_sptr Pass_Through::get_right_block()
{
if(inverted_spectrum)
if (inverted_spectrum)
{
if(item_type_.compare("gr_complex") == 0)
if (item_type_.compare("gr_complex") == 0)
{
return conjugate_cc_;
}
else if(item_type_.compare("cshort") == 0)
else if (item_type_.compare("cshort") == 0)
{
return conjugate_sc_;
}
else if(item_type_.compare("cbyte") == 0)
else if (item_type_.compare("cbyte") == 0)
{
return conjugate_ic_;
}

8
src/algorithms/libs/pass_through.h
View File

@@ -51,9 +51,9 @@ class Pass_Through : public GNSSBlockInterface
{
public:
Pass_Through(ConfigurationInterface* configuration,
std::string role,
unsigned int in_stream,
unsigned int out_stream);
std::string role,
unsigned int in_stream,
unsigned int out_stream);
virtual ~Pass_Through();
@@ -77,7 +77,7 @@ public:
{
return item_size_;
}
void connect(gr::top_block_sptr top_block) override;
void disconnect(gr::top_block_sptr top_block) override;
gr::basic_block_sptr get_left_block() override;

1706
src/algorithms/libs/rtklib/rtklib.h
View File

File diff suppressed because it is too large Load Diff

143
src/algorithms/libs/rtklib/rtklib_conversions.cc
View File

@@ -31,74 +31,73 @@
#include "rtklib_conversions.h"
#include "rtklib_rtkcmn.h"
obsd_t insert_obs_to_rtklib(obsd_t & rtklib_obs, const Gnss_Synchro & gnss_synchro, int week, int band)
obsd_t insert_obs_to_rtklib(obsd_t& rtklib_obs, const Gnss_Synchro& gnss_synchro, int week, int band)
{
rtklib_obs.D[band] = gnss_synchro.Carrier_Doppler_hz;
rtklib_obs.P[band] = gnss_synchro.Pseudorange_m;
rtklib_obs.L[band] = gnss_synchro.Carrier_phase_rads / PI_2;
switch(band)
{
switch (band)
{
case 0:
rtklib_obs.code[band] = static_cast<unsigned char>(CODE_L1C);
break;
case 1:
rtklib_obs.code[band] = static_cast<unsigned char>(CODE_L2S);
break;
rtklib_obs.code[band] = static_cast<unsigned char>(CODE_L2S);
break;
case 2:
rtklib_obs.code[band] = static_cast<unsigned char>(CODE_L5X);
break;
}
rtklib_obs.code[band] = static_cast<unsigned char>(CODE_L5X);
break;
}
double CN0_dB_Hz_est = gnss_synchro.CN0_dB_hz;
if (CN0_dB_Hz_est > 63.75) CN0_dB_Hz_est = 63.75;
if (CN0_dB_Hz_est < 0.0) CN0_dB_Hz_est = 0.0;
unsigned char CN0_dB_Hz = static_cast<unsigned char>(std::round(CN0_dB_Hz_est / 0.25 ));
unsigned char CN0_dB_Hz = static_cast<unsigned char>(std::round(CN0_dB_Hz_est / 0.25));
rtklib_obs.SNR[band] = CN0_dB_Hz;
//Galileo is the third satellite system for RTKLIB, so, add the required offset to discriminate Galileo ephemeris
switch(gnss_synchro.System)
{
switch (gnss_synchro.System)
{
case 'G':
rtklib_obs.sat = gnss_synchro.PRN;
break;
case 'E':
rtklib_obs.sat = gnss_synchro.PRN+NSATGPS+NSATGLO;
rtklib_obs.sat = gnss_synchro.PRN + NSATGPS + NSATGLO;
break;
case 'R':
rtklib_obs.sat = gnss_synchro.PRN+NSATGPS;
rtklib_obs.sat = gnss_synchro.PRN + NSATGPS;
break;
default:
rtklib_obs.sat = gnss_synchro.PRN;
}
}
rtklib_obs.time = gpst2time(adjgpsweek(week), gnss_synchro.RX_time);
rtklib_obs.rcv = 1;
return rtklib_obs;
}
geph_t eph_to_rtklib(const Glonass_Gnav_Ephemeris & glonass_gnav_eph, const Glonass_Gnav_Utc_Model & gnav_clock_model)
geph_t eph_to_rtklib(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, const Glonass_Gnav_Utc_Model& gnav_clock_model)
{
double week, sec;
int adj_week;
geph_t rtklib_sat = {0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0.0, 0.0, 0.0}, {0.0, 0.0,
0.0}, {0.0, 0.0, 0.0}, 0.0, 0.0, 0.0};
double week, sec;
int adj_week;
geph_t rtklib_sat = {0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}, 0.0, 0.0, 0.0};
rtklib_sat.sat = glonass_gnav_eph.i_satellite_slot_number + NSATGPS; /* satellite number */
rtklib_sat.iode = static_cast<int>(glonass_gnav_eph.d_t_b); /* IODE (0-6 bit of tb field) */
rtklib_sat.frq = glonass_gnav_eph.i_satellite_freq_channel; /* satellite frequency number */
rtklib_sat.svh = glonass_gnav_eph.d_l3rd_n; /* satellite health*/
rtklib_sat.sva = static_cast<int>(glonass_gnav_eph.d_F_T); /* satellite accuracy*/
rtklib_sat.age = static_cast<int>(glonass_gnav_eph.d_E_n); /* satellite age*/
rtklib_sat.pos[0] = glonass_gnav_eph.d_Xn*1000; /* satellite position (ecef) (m) */
rtklib_sat.pos[1] = glonass_gnav_eph.d_Yn*1000; /* satellite position (ecef) (m) */
rtklib_sat.pos[2] = glonass_gnav_eph.d_Zn*1000; /* satellite position (ecef) (m) */
rtklib_sat.vel[0] = glonass_gnav_eph.d_VXn*1000; /* satellite velocity (ecef) (m/s) */
rtklib_sat.vel[1] = glonass_gnav_eph.d_VYn*1000; /* satellite velocity (ecef) (m/s) */
rtklib_sat.vel[2] = glonass_gnav_eph.d_VZn*1000; /* satellite velocity (ecef) (m/s) */
rtklib_sat.acc[0] = glonass_gnav_eph.d_AXn*1000; /* satellite acceleration (ecef) (m/s^2) */
rtklib_sat.acc[1] = glonass_gnav_eph.d_AYn*1000; /* satellite acceleration (ecef) (m/s^2) */
rtklib_sat.acc[2] = glonass_gnav_eph.d_AZn*1000; /* satellite acceleration (ecef) (m/s^2) */
rtklib_sat.taun = glonass_gnav_eph.d_tau_n; /* SV clock bias (s) */
rtklib_sat.gamn = glonass_gnav_eph.d_gamma_n; /* SV relative freq bias */
rtklib_sat.age = static_cast<int>(glonass_gnav_eph.d_Delta_tau_n); /* delay between L1 and L2 (s) */
rtklib_sat.sat = glonass_gnav_eph.i_satellite_slot_number + NSATGPS; /* satellite number */
rtklib_sat.iode = static_cast<int>(glonass_gnav_eph.d_t_b); /* IODE (0-6 bit of tb field) */
rtklib_sat.frq = glonass_gnav_eph.i_satellite_freq_channel; /* satellite frequency number */
rtklib_sat.svh = glonass_gnav_eph.d_l3rd_n; /* satellite health*/
rtklib_sat.sva = static_cast<int>(glonass_gnav_eph.d_F_T); /* satellite accuracy*/
rtklib_sat.age = static_cast<int>(glonass_gnav_eph.d_E_n); /* satellite age*/
rtklib_sat.pos[0] = glonass_gnav_eph.d_Xn * 1000; /* satellite position (ecef) (m) */
rtklib_sat.pos[1] = glonass_gnav_eph.d_Yn * 1000; /* satellite position (ecef) (m) */
rtklib_sat.pos[2] = glonass_gnav_eph.d_Zn * 1000; /* satellite position (ecef) (m) */
rtklib_sat.vel[0] = glonass_gnav_eph.d_VXn * 1000; /* satellite velocity (ecef) (m/s) */
rtklib_sat.vel[1] = glonass_gnav_eph.d_VYn * 1000; /* satellite velocity (ecef) (m/s) */
rtklib_sat.vel[2] = glonass_gnav_eph.d_VZn * 1000; /* satellite velocity (ecef) (m/s) */
rtklib_sat.acc[0] = glonass_gnav_eph.d_AXn * 1000; /* satellite acceleration (ecef) (m/s^2) */
rtklib_sat.acc[1] = glonass_gnav_eph.d_AYn * 1000; /* satellite acceleration (ecef) (m/s^2) */
rtklib_sat.acc[2] = glonass_gnav_eph.d_AZn * 1000; /* satellite acceleration (ecef) (m/s^2) */
rtklib_sat.taun = glonass_gnav_eph.d_tau_n; /* SV clock bias (s) */
rtklib_sat.gamn = glonass_gnav_eph.d_gamma_n; /* SV relative freq bias */
rtklib_sat.age = static_cast<int>(glonass_gnav_eph.d_Delta_tau_n); /* delay between L1 and L2 (s) */
// Time expressed in GPS Time but using RTKLib format
glonass_gnav_eph.glot_to_gpst(glonass_gnav_eph.d_t_b, gnav_clock_model.d_tau_c, gnav_clock_model.d_tau_gps, &week, &sec);
@@ -114,12 +113,12 @@ geph_t eph_to_rtklib(const Glonass_Gnav_Ephemeris & glonass_gnav_eph, const Glon
}
eph_t eph_to_rtklib(const Galileo_Ephemeris & gal_eph)
eph_t eph_to_rtklib(const Galileo_Ephemeris& gal_eph)
{
eph_t rtklib_sat = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, 0.0, 0.0 };
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, 0.0, 0.0};
//Galileo is the third satellite system for RTKLIB, so, add the required offset to discriminate Galileo ephemeris
rtklib_sat.sat = gal_eph.i_satellite_PRN+NSATGPS+NSATGLO;
rtklib_sat.sat = gal_eph.i_satellite_PRN + NSATGPS + NSATGLO;
rtklib_sat.A = gal_eph.A_1 * gal_eph.A_1;
rtklib_sat.M0 = gal_eph.M0_1;
rtklib_sat.deln = gal_eph.delta_n_3;
@@ -129,8 +128,8 @@ eph_t eph_to_rtklib(const Galileo_Ephemeris & gal_eph)
rtklib_sat.i0 = gal_eph.i_0_2;
rtklib_sat.idot = gal_eph.iDot_2;
rtklib_sat.e = gal_eph.e_1;
rtklib_sat.Adot = 0; //only in CNAV;
rtklib_sat.ndot = 0; //only in CNAV;
rtklib_sat.Adot = 0; //only in CNAV;
rtklib_sat.ndot = 0; //only in CNAV;
rtklib_sat.week = adjgpsweek(gal_eph.WN_5); /* week of tow */
rtklib_sat.cic = gal_eph.C_ic_4;
@@ -154,8 +153,16 @@ eph_t eph_to_rtklib(const Galileo_Ephemeris & gal_eph)
double tow, toc;
tow = time2gpst(rtklib_sat.ttr, &rtklib_sat.week);
toc = time2gpst(rtklib_sat.toc, NULL);
if (rtklib_sat.toes < tow - 302400.0) {rtklib_sat.week++; tow -= 604800.0;}
else if (rtklib_sat.toes > tow + 302400.0) {rtklib_sat.week--; tow += 604800.0;}
if (rtklib_sat.toes < tow - 302400.0)
{
rtklib_sat.week++;
tow -= 604800.0;
}
else if (rtklib_sat.toes > tow + 302400.0)
{
rtklib_sat.week--;
tow += 604800.0;
}
rtklib_sat.toe = gpst2time(rtklib_sat.week, rtklib_sat.toes);
rtklib_sat.toc = gpst2time(rtklib_sat.week, toc);
rtklib_sat.ttr = gpst2time(rtklib_sat.week, tow);
@@ -164,10 +171,10 @@ eph_t eph_to_rtklib(const Galileo_Ephemeris & gal_eph)
}
eph_t eph_to_rtklib(const Gps_Ephemeris & gps_eph)
eph_t eph_to_rtklib(const Gps_Ephemeris& gps_eph)
{
eph_t rtklib_sat = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, 0.0, 0.0 };
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, 0.0, 0.0};
rtklib_sat.sat = gps_eph.i_satellite_PRN;
rtklib_sat.A = gps_eph.d_sqrt_A * gps_eph.d_sqrt_A;
rtklib_sat.M0 = gps_eph.d_M_0;
@@ -178,8 +185,8 @@ eph_t eph_to_rtklib(const Gps_Ephemeris & gps_eph)
rtklib_sat.i0 = gps_eph.d_i_0;
rtklib_sat.idot = gps_eph.d_IDOT;
rtklib_sat.e = gps_eph.d_e_eccentricity;
rtklib_sat.Adot = 0; //only in CNAV;
rtklib_sat.ndot = 0; //only in CNAV;
rtklib_sat.Adot = 0; //only in CNAV;
rtklib_sat.ndot = 0; //only in CNAV;
rtklib_sat.week = adjgpsweek(gps_eph.i_GPS_week); /* week of tow */
rtklib_sat.cic = gps_eph.d_Cic;
@@ -203,8 +210,16 @@ eph_t eph_to_rtklib(const Gps_Ephemeris & gps_eph)
double tow, toc;
tow = time2gpst(rtklib_sat.ttr, &rtklib_sat.week);
toc = time2gpst(rtklib_sat.toc, NULL);
if (rtklib_sat.toes < tow - 302400.0) {rtklib_sat.week++; tow -= 604800.0;}
else if (rtklib_sat.toes > tow + 302400.0) {rtklib_sat.week--; tow += 604800.0;}
if (rtklib_sat.toes < tow - 302400.0)
{
rtklib_sat.week++;
tow -= 604800.0;
}
else if (rtklib_sat.toes > tow + 302400.0)
{
rtklib_sat.week--;
tow += 604800.0;
}
rtklib_sat.toe = gpst2time(rtklib_sat.week, rtklib_sat.toes);
rtklib_sat.toc = gpst2time(rtklib_sat.week, toc);
rtklib_sat.ttr = gpst2time(rtklib_sat.week, tow);
@@ -213,26 +228,26 @@ eph_t eph_to_rtklib(const Gps_Ephemeris & gps_eph)
}
eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris & gps_cnav_eph)
eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris& gps_cnav_eph)
{
eph_t rtklib_sat = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, 0.0, 0.0 };
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, 0.0, 0.0};
rtklib_sat.sat = gps_cnav_eph.i_satellite_PRN;
const double A_REF = 26559710.0; // See IS-GPS-200H, pp. 170
const double A_REF = 26559710.0; // See IS-GPS-200H, pp. 170
rtklib_sat.A = A_REF + gps_cnav_eph.d_DELTA_A;
rtklib_sat.M0 = gps_cnav_eph.d_M_0;
rtklib_sat.deln = gps_cnav_eph.d_Delta_n;
rtklib_sat.OMG0 = gps_cnav_eph.d_OMEGA0;
// Compute the angle between the ascending node and the Greenwich meridian
const double OMEGA_DOT_REF = -2.6e-9; // semicircles / s, see IS-GPS-200H pp. 164
const double OMEGA_DOT_REF = -2.6e-9; // semicircles / s, see IS-GPS-200H pp. 164
double d_OMEGA_DOT = OMEGA_DOT_REF * PI + gps_cnav_eph.d_DELTA_OMEGA_DOT;
rtklib_sat.OMGd = d_OMEGA_DOT;
rtklib_sat.omg = gps_cnav_eph.d_OMEGA;
rtklib_sat.i0 = gps_cnav_eph.d_i_0;
rtklib_sat.idot = gps_cnav_eph.d_IDOT;
rtklib_sat.e = gps_cnav_eph.d_e_eccentricity;
rtklib_sat.Adot = gps_cnav_eph.d_A_DOT; //only in CNAV;
rtklib_sat.ndot = gps_cnav_eph.d_DELTA_DOT_N; //only in CNAV;
rtklib_sat.Adot = gps_cnav_eph.d_A_DOT; //only in CNAV;
rtklib_sat.ndot = gps_cnav_eph.d_DELTA_DOT_N; //only in CNAV;
rtklib_sat.week = adjgpsweek(gps_cnav_eph.i_GPS_week); /* week of tow */
rtklib_sat.cic = gps_cnav_eph.d_Cic;
@@ -249,15 +264,23 @@ eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris & gps_cnav_eph)
rtklib_sat.tgd[2] = 0;
rtklib_sat.tgd[3] = 0;
rtklib_sat.toes = gps_cnav_eph.d_Toe1;
rtklib_sat.toc = gpst2time(rtklib_sat.week,gps_cnav_eph.d_Toc);
rtklib_sat.ttr = gpst2time(rtklib_sat.week,gps_cnav_eph.d_TOW);
rtklib_sat.toc = gpst2time(rtklib_sat.week, gps_cnav_eph.d_Toc);
rtklib_sat.ttr = gpst2time(rtklib_sat.week, gps_cnav_eph.d_TOW);
/* adjustment for week handover */
double tow, toc;
tow = time2gpst(rtklib_sat.ttr, &rtklib_sat.week);
toc = time2gpst(rtklib_sat.toc, NULL);
if (rtklib_sat.toes < tow - 302400.0) {rtklib_sat.week++; tow -= 604800.0;}
else if (rtklib_sat.toes > tow + 302400.0) {rtklib_sat.week--; tow += 604800.0;}
if (rtklib_sat.toes < tow - 302400.0)
{
rtklib_sat.week++;
tow -= 604800.0;
}
else if (rtklib_sat.toes > tow + 302400.0)
{
rtklib_sat.week--;
tow += 604800.0;
}
rtklib_sat.toe = gpst2time(rtklib_sat.week, rtklib_sat.toes);
rtklib_sat.toc = gpst2time(rtklib_sat.week, toc);
rtklib_sat.ttr = gpst2time(rtklib_sat.week, tow);

10
src/algorithms/libs/rtklib/rtklib_conversions.h
View File

@@ -39,16 +39,16 @@
#include "glonass_gnav_ephemeris.h"
#include "glonass_gnav_utc_model.h"
eph_t eph_to_rtklib(const Galileo_Ephemeris & gal_eph);
eph_t eph_to_rtklib(const Gps_Ephemeris & gps_eph);
eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris & gps_cnav_eph);
eph_t eph_to_rtklib(const Galileo_Ephemeris& gal_eph);
eph_t eph_to_rtklib(const Gps_Ephemeris& gps_eph);
eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris& gps_cnav_eph);
/*!
* \brief Transforms a Glonass_Gnav_Ephemeris to its RTKLIB counterpart
* \param glonass_gnav_eph GLONASS GNAV Ephemeris structure
* \return Ephemeris structure for RTKLIB parsing
*/
geph_t eph_to_rtklib(const Glonass_Gnav_Ephemeris & glonass_gnav_eph, const Glonass_Gnav_Utc_Model & gnav_clock_model);
geph_t eph_to_rtklib(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, const Glonass_Gnav_Utc_Model& gnav_clock_model);
obsd_t insert_obs_to_rtklib(obsd_t & rtklib_obs, const Gnss_Synchro & gnss_synchro, int week, int band);
obsd_t insert_obs_to_rtklib(obsd_t& rtklib_obs, const Gnss_Synchro& gnss_synchro, int week, int band);
#endif /* GNSS_SDR_RTKLIB_CONVERSIONS_H_ */

233
src/algorithms/libs/rtklib/rtklib_ephemeris.cc
View File

@@ -57,41 +57,40 @@
/* constants ------------------------------------------------------*/
const double RE_GLO = 6378136.0; /* radius of earth (m) ref [2] */
const double MU_GPS = 3.9860050e14; /* gravitational constant ref [1] */
const double MU_GLO = 3.9860044e14; /* gravitational constant ref [2] */
const double MU_GAL = 3.986004418e14; /* earth gravitational constant ref [7] */
const double MU_BDS = 3.986004418e14; /* earth gravitational constant ref [9] */
const double J2_GLO = 1.0826257e-3; /* 2nd zonal harmonic of geopot ref [2] */
const double RE_GLO = 6378136.0; /* radius of earth (m) ref [2] */
const double MU_GPS = 3.9860050e14; /* gravitational constant ref [1] */
const double MU_GLO = 3.9860044e14; /* gravitational constant ref [2] */
const double MU_GAL = 3.986004418e14; /* earth gravitational constant ref [7] */
const double MU_BDS = 3.986004418e14; /* earth gravitational constant ref [9] */
const double J2_GLO = 1.0826257e-3; /* 2nd zonal harmonic of geopot ref [2] */
const double OMGE_GLO = 7.292115e-5; /* earth angular velocity (rad/s) ref [2] */
const double OMGE_GAL = 7.2921151467e-5; /* earth angular velocity (rad/s) ref [7] */
const double OMGE_BDS = 7.292115e-5; /* earth angular velocity (rad/s) ref [9] */
const double OMGE_GLO = 7.292115e-5; /* earth angular velocity (rad/s) ref [2] */
const double OMGE_GAL = 7.2921151467e-5; /* earth angular velocity (rad/s) ref [7] */
const double OMGE_BDS = 7.292115e-5; /* earth angular velocity (rad/s) ref [9] */
const double SIN_5 = -0.0871557427476582; /* sin(-5.0 deg) */
const double COS_5 = 0.9961946980917456; /* cos(-5.0 deg) */
const double COS_5 = 0.9961946980917456; /* cos(-5.0 deg) */
const double ERREPH_GLO = 5.0; /* error of glonass ephemeris (m) */
const double TSTEP = 60.0; /* integration step glonass ephemeris (s) */
const double RTOL_KEPLER = 1e-13; /* relative tolerance for Kepler equation */
const double ERREPH_GLO = 5.0; /* error of glonass ephemeris (m) */
const double TSTEP = 60.0; /* integration step glonass ephemeris (s) */
const double RTOL_KEPLER = 1e-13; /* relative tolerance for Kepler equation */
const double DEFURASSR = 0.15; /* default accurary of ssr corr (m) */
const double MAXECORSSR = 10.0; /* max orbit correction of ssr (m) */
const double MAXCCORSSR = 1e-6 * SPEED_OF_LIGHT; /* max clock correction of ssr (m) */
const double MAXAGESSR = 90.0; /* max age of ssr orbit and clock (s) */
const double MAXAGESSR_HRCLK = 10.0; /* max age of ssr high-rate clock (s) */
const double STD_BRDCCLK = 30.0; /* error of broadcast clock (m) */
const double DEFURASSR = 0.15; /* default accurary of ssr corr (m) */
const double MAXECORSSR = 10.0; /* max orbit correction of ssr (m) */
const double MAXCCORSSR = 1e-6 * SPEED_OF_LIGHT; /* max clock correction of ssr (m) */
const double MAXAGESSR = 90.0; /* max age of ssr orbit and clock (s) */
const double MAXAGESSR_HRCLK = 10.0; /* max age of ssr high-rate clock (s) */
const double STD_BRDCCLK = 30.0; /* error of broadcast clock (m) */
const int MAX_ITER_KEPLER = 30; /* max number of iteration of Kelpler */
const int MAX_ITER_KEPLER = 30; /* max number of iteration of Kelpler */
/* variance by ura ephemeris (ref [1] 20.3.3.3.1.1) --------------------------*/
double var_uraeph(int ura)
{
const double ura_value[] = {
2.4, 3.4, 4.85, 6.85, 9.65, 13.65, 24.0, 48.0, 96.0, 192.0, 384.0, 768.0, 1536.0,
3072.0, 6144.0
};
2.4, 3.4, 4.85, 6.85, 9.65, 13.65, 24.0, 48.0, 96.0, 192.0, 384.0, 768.0, 1536.0,
3072.0, 6144.0};
return ura < 0 || 14 < ura ? std::pow(6144.0, 2.0) : std::pow(ura_value[ura], 2.0);
}
@@ -140,13 +139,13 @@ void alm2pos(gtime_t time, const alm_t *alm, double *rs, double *dts)
}
if (n >= MAX_ITER_KEPLER)
{
trace(2, "alm2pos: kepler iteration overflow sat=%2d\n", alm->sat);
trace(2, "alm2pos: kepler iteration overflow sat=%2d\n", alm->sat);
return;
}
sinE = sin(E);
cosE = cos(E);
u = atan2(sqrt(1.0 - alm->e * alm->e) * sinE, cosE - alm->e) + alm->omg;
r = alm->A * (1.0 - alm->e*cosE);
r = alm->A * (1.0 - alm->e * cosE);
i = alm->i0;
O = alm->OMG0 + (alm->OMGd - DEFAULT_OMEGA_EARTH_DOT) * tk - DEFAULT_OMEGA_EARTH_DOT * alm->toas;
x = r * cos(u);
@@ -182,7 +181,7 @@ double eph2clk(gtime_t time, const eph_t *eph)
{
t -= eph->f0 + eph->f1 * t + eph->f2 * t * t;
}
return eph->f0 + eph->f1 * t + eph->f2 * t *t;
return eph->f0 + eph->f1 * t + eph->f2 * t * t;
}
@@ -200,7 +199,7 @@ double eph2clk(gtime_t time, const eph_t *eph)
* (tgd or bgd)
*-----------------------------------------------------------------------------*/
void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts,
double *var)
double *var)
{
double tk, M, E, Ek, sinE, cosE, u, r, i, O, sin2u, cos2u, x, y, sinO, cosO, cosi, mu, omge;
double xg, yg, zg, sino, coso;
@@ -213,14 +212,23 @@ void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts,
rs[0] = rs[1] = rs[2] = *dts = *var = 0.0;
return;
}
tk = timediff(time , eph->toe);
tk = timediff(time, eph->toe);
switch ((sys = satsys(eph->sat, &prn)))
{
case SYS_GAL: mu = MU_GAL; omge = OMGE_GAL; break;
case SYS_BDS: mu = MU_BDS; omge = OMGE_BDS; break;
default: mu = MU_GPS; omge = DEFAULT_OMEGA_EARTH_DOT; break;
}
{
case SYS_GAL:
mu = MU_GAL;
omge = OMGE_GAL;
break;
case SYS_BDS:
mu = MU_BDS;
omge = OMGE_BDS;
break;
default:
mu = MU_GPS;
omge = DEFAULT_OMEGA_EARTH_DOT;
break;
}
M = eph->M0 + (sqrt(mu / (eph->A * eph->A * eph->A)) + eph->deln) * tk;
for (n = 0, E = M, Ek = 0.0; fabs(E - Ek) > RTOL_KEPLER && n < MAX_ITER_KEPLER; n++)
@@ -238,10 +246,11 @@ void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts,
trace(4, "kepler: sat=%2d e=%8.5f n=%2d del=%10.3e\n", eph->sat, eph->e, n, E - Ek);
u = atan2(sqrt(1.0 - eph->e*eph->e) * sinE, cosE-eph->e) + eph->omg;
u = atan2(sqrt(1.0 - eph->e * eph->e) * sinE, cosE - eph->e) + eph->omg;
r = eph->A * (1.0 - eph->e * cosE);
i = eph->i0 + eph->idot * tk;
sin2u = sin(2.0 * u); cos2u = cos(2.0 * u);
sin2u = sin(2.0 * u);
cos2u = cos(2.0 * u);
u += eph->cus * sin2u + eph->cuc * cos2u;
r += eph->crs * sin2u + eph->crc * cos2u;
i += eph->cis * sin2u + eph->cic * cos2u;
@@ -260,7 +269,7 @@ void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts,
zg = y * sin(i);
sino = sin(omge * tk);
coso = cos(omge * tk);
rs[0] = xg * coso + yg * sino * COS_5 + zg * sino * SIN_5;
rs[0] = xg * coso + yg * sino * COS_5 + zg * sino * SIN_5;
rs[1] = -xg * sino + yg * coso * COS_5 + zg * coso * SIN_5;
rs[2] = -yg * SIN_5 + zg * COS_5;
}
@@ -270,14 +279,14 @@ void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts,
sinO = sin(O);
cosO = cos(O);
rs[0] = x * cosO - y * cosi * sinO;
rs[1] = x * sinO + y *cosi * cosO;
rs[1] = x * sinO + y * cosi * cosO;
rs[2] = y * sin(i);
}
tk = timediff(time, eph->toc);
*dts = eph->f0 + eph->f1 * tk + eph->f2 * tk * tk;
/* relativity correction */
*dts -= 2.0 * sqrt(mu * eph->A) * eph-> e* sinE / std::pow(SPEED_OF_LIGHT, 2.0);
*dts -= 2.0 * sqrt(mu * eph->A) * eph->e * sinE / std::pow(SPEED_OF_LIGHT, 2.0);
/* position and clock error variance */
*var = var_uraeph(eph->sva);
@@ -296,10 +305,11 @@ void deq(const double *x, double *xdot, const double *acc)
}
/* ref [2] A.3.1.2 with bug fix for xdot[4],xdot[5] */
a = 1.5 * J2_GLO * MU_GLO * std::pow(RE_GLO, 2.0) / r2 / r3; /* 3/2*J2*mu*Ae^2/r^5 */
b = 5.0 * x[2] * x[2] / r2; /* 5*z^2/r^2 */
c = -MU_GLO / r3 - a * (1.0 - b); /* -mu/r^3-a(1-b) */
b = 5.0 * x[2] * x[2] / r2; /* 5*z^2/r^2 */
c = -MU_GLO / r3 - a * (1.0 - b); /* -mu/r^3-a(1-b) */
xdot[0] = x[3];
xdot[1] = x[4]; xdot[2] = x[5];
xdot[1] = x[4];
xdot[2] = x[5];
xdot[3] = (c + omg2) * x[0] + 2.0 * OMGE_GLO * x[4] + acc[0];
xdot[4] = (c + omg2) * x[1] - 2.0 * OMGE_GLO * x[3] + acc[1];
xdot[5] = (c - 2.0 * a) * x[2] + acc[2];
@@ -312,9 +322,12 @@ void glorbit(double t, double *x, const double *acc)
double k1[6], k2[6], k3[6], k4[6], w[6];
int i;
deq(x, k1, acc); for (i = 0; i< 6; i++) w[i] = x[i] + k1[i] * t / 2.0;
deq(w, k2, acc); for (i = 0; i < 6; i++) w[i] = x[i] + k2[i] * t / 2.0;
deq(w, k3, acc); for (i = 0; i < 6; i++) w[i] = x[i] + k3[i] * t;
deq(x, k1, acc);
for (i = 0; i < 6; i++) w[i] = x[i] + k1[i] * t / 2.0;
deq(w, k2, acc);
for (i = 0; i < 6; i++) w[i] = x[i] + k2[i] * t / 2.0;
deq(w, k3, acc);
for (i = 0; i < 6; i++) w[i] = x[i] + k3[i] * t;
deq(w, k4, acc);
for (i = 0; i < 6; i++) x[i] += (k1[i] + 2.0 * k2[i] + 2.0 * k3[i] + k4[i]) * t / 6.0;
}
@@ -355,7 +368,7 @@ double geph2clk(gtime_t time, const geph_t *geph)
* notes : see ref [2]
*-----------------------------------------------------------------------------*/
void geph2pos(gtime_t time, const geph_t *geph, double *rs, double *dts,
double *var)
double *var)
{
double t, tt, x[6];
int i;
@@ -368,10 +381,10 @@ void geph2pos(gtime_t time, const geph_t *geph, double *rs, double *dts,
for (i = 0; i < 3; i++)
{
x[i ] = geph->pos[i];
x[i+3] = geph->vel[i];
x[i] = geph->pos[i];
x[i + 3] = geph->vel[i];
}
for (tt = t < 0.0 ? - TSTEP : TSTEP; fabs(t) > 1e-9; t -= tt)
for (tt = t < 0.0 ? -TSTEP : TSTEP; fabs(t) > 1e-9; t -= tt)
{
if (fabs(t) < TSTEP) tt = t;
glorbit(tt, x, geph->acc);
@@ -400,7 +413,7 @@ double seph2clk(gtime_t time, const seph_t *seph)
for (i = 0; i < 2; i++)
{
t-=seph->af0 + seph->af1 * t;
t -= seph->af0 + seph->af1 * t;
}
return seph->af0 + seph->af1 * t;
}
@@ -417,7 +430,7 @@ double seph2clk(gtime_t time, const seph_t *seph)
* notes : see ref [3]
*-----------------------------------------------------------------------------*/
void seph2pos(gtime_t time, const seph_t *seph, double *rs, double *dts,
double *var)
double *var)
{
double t;
int i;
@@ -445,12 +458,20 @@ eph_t *seleph(gtime_t time, int sat, int iode, const nav_t *nav)
trace(4, "seleph : time=%s sat=%2d iode=%d\n", time_str(time, 3), sat, iode);
switch (satsys(sat, NULL))
{
case SYS_QZS: tmax = MAXDTOE_QZS + 1.0; break;
case SYS_GAL: tmax = MAXDTOE_GAL + 1.0; break;
case SYS_BDS: tmax = MAXDTOE_BDS + 1.0; break;
default: tmax = MAXDTOE + 1.0; break;
}
{
case SYS_QZS:
tmax = MAXDTOE_QZS + 1.0;
break;
case SYS_GAL:
tmax = MAXDTOE_GAL + 1.0;
break;
case SYS_BDS:
tmax = MAXDTOE_BDS + 1.0;
break;
default:
tmax = MAXDTOE + 1.0;
break;
}
tmin = tmax + 1.0;
for (i = 0; i < nav->n; i++)
@@ -459,12 +480,16 @@ eph_t *seleph(gtime_t time, int sat, int iode, const nav_t *nav)
if (iode >= 0 && nav->eph[i].iode != iode) continue;
if ((t = fabs(timediff(nav->eph[i].toe, time))) > tmax) continue;
if (iode >= 0) return nav->eph + i;
if (t <= tmin) {j = i; tmin = t;} /* toe closest to time */
if (t <= tmin)
{
j = i;
tmin = t;
} /* toe closest to time */
}
if (iode >= 0 || j<0)
if (iode >= 0 || j < 0)
{
trace(3, "no broadcast ephemeris: %s sat=%2d iode=%3d\n", time_str(time, 0),
sat, iode);
sat, iode);
return NULL;
}
return nav->eph + j;
@@ -485,12 +510,16 @@ geph_t *selgeph(gtime_t time, int sat, int iode, const nav_t *nav)
if (iode >= 0 && nav->geph[i].iode != iode) continue;
if ((t = fabs(timediff(nav->geph[i].toe, time))) > tmax) continue;
if (iode >= 0) return nav->geph + i;
if (t <= tmin) {j = i; tmin = t;} /* toe closest to time */
if (t <= tmin)
{
j = i;
tmin = t;
} /* toe closest to time */
}
if (iode >= 0 || j < 0)
{
trace(3, "no glonass ephemeris : %s sat=%2d iode=%2d\n", time_str(time, 0),
sat, iode);
sat, iode);
return NULL;
}
return nav->geph + j;
@@ -509,7 +538,11 @@ seph_t *selseph(gtime_t time, int sat, const nav_t *nav)
{
if (nav->seph[i].sat != sat) continue;
if ((t = fabs(timediff(nav->seph[i].t0, time))) > tmax) continue;
if (t <= tmin) {j = i; tmin = t;} /* toe closest to time */
if (t <= tmin)
{
j = i;
tmin = t;
} /* toe closest to time */
}
if (j < 0)
{
@@ -522,9 +555,9 @@ seph_t *selseph(gtime_t time, int sat, const nav_t *nav)
/* satellite clock with broadcast ephemeris ----------------------------------*/
int ephclk(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
double *dts)
double *dts)
{
eph_t *eph;
eph_t *eph;
geph_t *geph;
seph_t *seph;
int sys;
@@ -548,7 +581,8 @@ int ephclk(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
if (!(seph = selseph(teph, sat, nav))) return 0;
*dts = seph2clk(time, seph);
}
else return 0;
else
return 0;
return 1;
}
@@ -556,9 +590,9 @@ int ephclk(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
/* satellite position and clock by broadcast ephemeris -----------------------*/
int ephpos(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
int iode, double *rs, double *dts, double *var, int *svh)
int iode, double *rs, double *dts, double *var, int *svh)
{
eph_t *eph;
eph_t *eph;
geph_t *geph;
seph_t *seph;
double rst[3], dtst[1], tt = 1e-3;
@@ -596,7 +630,8 @@ int ephpos(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
seph2pos(time, seph, rst, dtst, var);
*svh = seph->svh;
}
else return 0;
else
return 0;
/* satellite velocity and clock drift by differential approx */
for (i = 0; i < 3; i++) rs[i + 3] = (rst[i] - rs[i]) / tt;
@@ -608,7 +643,7 @@ int ephpos(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
/* satellite position and clock with sbas correction -------------------------*/
int satpos_sbas(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
double *rs, double *dts, double *var, int *svh)
double *rs, double *dts, double *var, int *svh)
{
const sbssatp_t *sbs;
int i;
@@ -640,7 +675,7 @@ int satpos_sbas(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
/* satellite position and clock with ssr correction --------------------------*/
int satpos_ssr(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
int opt, double *rs, double *dts, double *var, int *svh)
int opt, double *rs, double *dts, double *var, int *svh)
{
const ssr_t *ssr;
eph_t *eph;
@@ -665,7 +700,7 @@ int satpos_ssr(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
if (ssr->iod[0] != ssr->iod[1])
{
trace(2, "inconsist ssr correction: %s sat=%2d iod=%d %d\n",
time_str(time, 0), sat, ssr->iod[0], ssr->iod[1]);
time_str(time, 0), sat, ssr->iod[0], ssr->iod[1]);
*svh = -1;
return 0;
}
@@ -677,7 +712,7 @@ int satpos_ssr(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
if (fabs(t1) > MAXAGESSR || fabs(t2) > MAXAGESSR)
{
trace(2, "age of ssr error: %s sat=%2d t=%.0f %.0f\n", time_str(time, 0),
sat, t1, t2);
sat, t1, t2);
*svh = -1;
return 0;
}
@@ -695,7 +730,7 @@ int satpos_ssr(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
if (norm_rtk(deph, 3) > MAXECORSSR || fabs(dclk) > MAXCCORSSR)
{
trace(3, "invalid ssr correction: %s deph=%.1f dclk=%.1f\n",
time_str(time, 0), norm_rtk(deph, 3), dclk);
time_str(time, 0), norm_rtk(deph, 3), dclk);
*svh = -1;
return 0;
}
@@ -709,9 +744,9 @@ int satpos_ssr(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
if (!(eph = seleph(teph, sat, ssr->iode, nav))) return 0;
/* satellite clock by clock parameters */
tk=timediff(time, eph->toc);
dts[0] = eph->f0 + eph->f1*tk + eph->f2 * tk * tk;
dts[1] = eph->f1 + 2.0*eph->f2 * tk;
tk = timediff(time, eph->toc);
dts[0] = eph->f0 + eph->f1 * tk + eph->f2 * tk * tk;
dts[1] = eph->f1 + 2.0 * eph->f2 * tk;
/* relativity correction */
dts[0] -= 2.0 * dot(rs, rs + 3, 3) / SPEED_OF_LIGHT / SPEED_OF_LIGHT;
@@ -742,7 +777,7 @@ int satpos_ssr(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
*var = var_urassr(ssr->ura);
trace(5, "satpos_ssr: %s sat=%2d deph=%6.3f %6.3f %6.3f er=%6.3f %6.3f %6.3f dclk=%6.3f var=%6.3f\n",
time_str(time, 2), sat, deph[0], deph[1], deph[2], er[0], er[1], er[2], dclk, *var);
time_str(time, 2), sat, deph[0], deph[1], deph[2], er[0], er[1], er[2], dclk, *var);
return 1;
}
@@ -765,25 +800,32 @@ int satpos_ssr(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
* satellite clock does not include code bias correction (tgd or bgd)
*-----------------------------------------------------------------------------*/
int satpos(gtime_t time, gtime_t teph, int sat, int ephopt,
const nav_t *nav, double *rs, double *dts, double *var,
int *svh)
const nav_t *nav, double *rs, double *dts, double *var,
int *svh)
{
trace(4, "satpos : time=%s sat=%2d ephopt=%d\n", time_str(time, 3), sat, ephopt);
*svh = 0;
switch (ephopt)
{
case EPHOPT_BRDC : return ephpos (time, teph, sat, nav, -1, rs, dts, var, svh);
case EPHOPT_SBAS : return satpos_sbas(time, teph, sat, nav, rs, dts, var, svh);
case EPHOPT_SSRAPC: return satpos_ssr (time, teph, sat, nav, 0, rs, dts, var, svh);
case EPHOPT_SSRCOM: return satpos_ssr (time, teph, sat, nav, 1, rs, dts, var, svh);
case EPHOPT_PREC :
if (!peph2pos(time, sat, nav, 1, rs, dts, var)) break; else return 1;
//TODO: enable lex
//case EPHOPT_LEX :
// if (!lexeph2pos(time, sat, nav, rs, dts, var)) break; else return 1;
}
{
case EPHOPT_BRDC:
return ephpos(time, teph, sat, nav, -1, rs, dts, var, svh);
case EPHOPT_SBAS:
return satpos_sbas(time, teph, sat, nav, rs, dts, var, svh);
case EPHOPT_SSRAPC:
return satpos_ssr(time, teph, sat, nav, 0, rs, dts, var, svh);
case EPHOPT_SSRCOM:
return satpos_ssr(time, teph, sat, nav, 1, rs, dts, var, svh);
case EPHOPT_PREC:
if (!peph2pos(time, sat, nav, 1, rs, dts, var))
break;
else
return 1;
//TODO: enable lex
//case EPHOPT_LEX :
// if (!lexeph2pos(time, sat, nav, rs, dts, var)) break; else return 1;
}
*svh = -1;
return 0;
}
@@ -814,7 +856,7 @@ int satpos(gtime_t time, gtime_t teph, int sat, int ephopt,
* signal transmission time
*-----------------------------------------------------------------------------*/
void satposs(gtime_t teph, const obsd_t *obs, int n, const nav_t *nav,
int ephopt, double *rs, double *dts, double *var, int *svh)
int ephopt, double *rs, double *dts, double *var, int *svh)
{
gtime_t time[MAXOBS] = {};
double dt, pr;
@@ -824,13 +866,14 @@ void satposs(gtime_t teph, const obsd_t *obs, int n, const nav_t *nav,
for (i = 0; i < n && i < MAXOBS; i++)
{
for (j = 0; j < 6; j++) rs [j + i * 6] = 0.0;
for (j = 0; j < 6; j++) rs[j + i * 6] = 0.0;
for (j = 0; j < 2; j++) dts[j + i * 2] = 0.0;
var[i] = 0.0;
svh[i] = 0;
/* search any pseudorange */
for (j = 0, pr = 0.0; j < NFREQ; j++) if ((pr = obs[i].P[j]) != 0.0) break;
for (j = 0, pr = 0.0; j < NFREQ; j++)
if ((pr = obs[i].P[j]) != 0.0) break;
if (j >= NFREQ)
{
@@ -866,7 +909,7 @@ void satposs(gtime_t teph, const obsd_t *obs, int n, const nav_t *nav,
for (i = 0; i < n && i < MAXOBS; i++)
{
trace(4, "%s sat=%2d rs=%13.3f %13.3f %13.3f dts=%12.3f var=%7.3f svh=%02X\n",
time_str(time[i], 6), obs[i].sat, rs[i * 6], rs[1 + i * 6], rs[2 + i * 6],
dts[i * 2] * 1e9, var[i], svh[i]);
time_str(time[i], 6), obs[i].sat, rs[i * 6], rs[1 + i * 6], rs[2 + i * 6],
dts[i * 2] * 1e9, var[i], svh[i]);
}
}

22
src/algorithms/libs/rtklib/rtklib_ephemeris.h
View File

@@ -62,36 +62,34 @@ double var_urassr(int ura);
void alm2pos(gtime_t time, const alm_t *alm, double *rs, double *dts);
double eph2clk(gtime_t time, const eph_t *eph);
void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts,
double *var);
double *var);
void deq(const double *x, double *xdot, const double *acc);
void glorbit(double t, double *x, const double *acc);
double geph2clk(gtime_t time, const geph_t *geph);
void geph2pos(gtime_t time, const geph_t *geph, double *rs, double *dts,
double *var);
double *var);
double seph2clk(gtime_t time, const seph_t *seph);
void seph2pos(gtime_t time, const seph_t *seph, double *rs, double *dts,
double *var);
double *var);
eph_t *seleph(gtime_t time, int sat, int iode, const nav_t *nav);
geph_t *selgeph(gtime_t time, int sat, int iode, const nav_t *nav);
seph_t *selseph(gtime_t time, int sat, const nav_t *nav);
int ephclk(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
double *dts);
double *dts);
//satellite position and clock by broadcast ephemeris
int ephpos(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
int iode, double *rs, double *dts, double *var, int *svh);
int iode, double *rs, double *dts, double *var, int *svh);
int satpos_sbas(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
double *rs, double *dts, double *var, int *svh);
double *rs, double *dts, double *var, int *svh);
int satpos_ssr(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
int opt, double *rs, double *dts, double *var, int *svh);
int opt, double *rs, double *dts, double *var, int *svh);
int satpos(gtime_t time, gtime_t teph, int sat, int ephopt,
const nav_t *nav, double *rs, double *dts, double *var,
int *svh);
const nav_t *nav, double *rs, double *dts, double *var,
int *svh);
void satposs(gtime_t teph, const obsd_t *obs, int n, const nav_t *nav,
int ephopt, double *rs, double *dts, double *var, int *svh);
int ephopt, double *rs, double *dts, double *var, int *svh);
#endif /* GNSS_SDR_RTKLIB_EPHEMERIS_H_ */

148
src/algorithms/libs/rtklib/rtklib_ionex.cc
View File

@@ -64,8 +64,8 @@
int getindex(double value, const double *range)
{
if (range[2] == 0.0) return 0;
if (range[1] > 0.0 && (value < range[0] || range[1]<value)) return -1;
if (range[1] < 0.0 && (value < range[1] || range[0]<value)) return -1;
if (range[1] > 0.0 && (value < range[0] || range[1] < value)) return -1;
if (range[1] < 0.0 && (value < range[1] || range[0] < value)) return -1;
return (int)floor((value - range[0]) / range[2] + 0.5);
}
@@ -87,7 +87,7 @@ int dataindex(int i, int j, int k, const int *ndata)
/* add tec data to navigation data -------------------------------------------*/
tec_t *addtec(const double *lats, const double *lons, const double *hgts,
double rb, nav_t *nav)
double rb, nav_t *nav)
{
tec_t *p, *nav_tec;
gtime_t time0 = {0, 0};
@@ -102,16 +102,18 @@ tec_t *addtec(const double *lats, const double *lons, const double *hgts,
if (nav->nt >= nav->ntmax)
{
nav->ntmax+=256;
if (!(nav_tec = (tec_t *)realloc(nav->tec, sizeof(tec_t)*nav->ntmax)))
nav->ntmax += 256;
if (!(nav_tec = (tec_t *)realloc(nav->tec, sizeof(tec_t) * nav->ntmax)))
{
trace(1, "readionex malloc error ntmax=%d\n", nav->ntmax);
free(nav->tec); nav->tec = NULL; nav->nt = nav->ntmax = 0;
free(nav->tec);
nav->tec = NULL;
nav->nt = nav->ntmax = 0;
return NULL;
}
nav->tec = nav_tec;
}
p = nav->tec+nav->nt;
p = nav->tec + nav->nt;
p->time = time0;
p->rb = rb;
for (i = 0; i < 3; i++)
@@ -124,14 +126,14 @@ tec_t *addtec(const double *lats, const double *lons, const double *hgts,
n = ndata[0] * ndata[1] * ndata[2];
if (!(p->data = (double *)malloc(sizeof(double) * n)) ||
!(p->rms = (float *)malloc(sizeof(float ) * n)))
!(p->rms = (float *)malloc(sizeof(float) * n)))
{
return NULL;
}
for (i = 0; i < n; i++)
{
p->data[i] = 0.0;
p->rms [i] = 0.0f;
p->rms[i] = 0.0f;
}
nav->nt++;
return p;
@@ -155,24 +157,26 @@ void readionexdcb(FILE *fp, double *dcb, double *rms)
if (strstr(label, "PRN / BIAS / RMS") == label)
{
strncpy(id, buff+3, 3); id[3] = '\0';
strncpy(id, buff + 3, 3);
id[3] = '\0';
if (!(sat = satid2no(id)))
{
trace(2, "ionex invalid satellite: %s\n", id);
continue;
}
dcb[sat-1] = str2num(buff, 6, 10);
rms[sat-1] = str2num(buff, 16, 10);
dcb[sat - 1] = str2num(buff, 6, 10);
rms[sat - 1] = str2num(buff, 16, 10);
}
else if (strstr(label, "END OF AUX DATA") == label) break;
else if (strstr(label, "END OF AUX DATA") == label)
break;
}
}
/* read ionex header ---------------------------------------------------------*/
double readionexh(FILE *fp, double *lats, double *lons, double *hgts,
double *rb, double *nexp, double *dcb, double *rms)
double *rb, double *nexp, double *dcb, double *rms)
{
double ver = 0.0;
char buff[1024], *label;
@@ -181,7 +185,6 @@ double readionexh(FILE *fp, double *lats, double *lons, double *hgts,
while (fgets(buff, sizeof(buff), fp))
{
if (strlen(buff) < 60) continue;
label = buff + 60;
@@ -195,20 +198,20 @@ double readionexh(FILE *fp, double *lats, double *lons, double *hgts,
}
else if (strstr(label, "HGT1 / HGT2 / DHGT") == label)
{
hgts[0] = str2num(buff, 2, 6);
hgts[1] = str2num(buff, 8, 6);
hgts[0] = str2num(buff, 2, 6);
hgts[1] = str2num(buff, 8, 6);
hgts[2] = str2num(buff, 14, 6);
}
else if (strstr(label, "LAT1 / LAT2 / DLAT") == label)
{
lats[0] = str2num(buff, 2, 6);
lats[1] = str2num(buff, 8, 6);
lats[0] = str2num(buff, 2, 6);
lats[1] = str2num(buff, 8, 6);
lats[2] = str2num(buff, 14, 6);
}
else if (strstr(label,"LON1 / LON2 / DLON") == label)
else if (strstr(label, "LON1 / LON2 / DLON") == label)
{
lons[0] = str2num(buff, 2, 6);
lons[1] = str2num(buff, 8, 6);
lons[0] = str2num(buff, 2, 6);
lons[1] = str2num(buff, 8, 6);
lons[2] = str2num(buff, 14, 6);
}
else if (strstr(label, "EXPONENT") == label)
@@ -216,7 +219,7 @@ double readionexh(FILE *fp, double *lats, double *lons, double *hgts,
*nexp = str2num(buff, 0, 6);
}
else if (strstr(label, "START OF AUX DATA") == label &&
strstr(buff, "DIFFERENTIAL CODE BIASES"))
strstr(buff, "DIFFERENTIAL CODE BIASES"))
{
readionexdcb(fp, dcb, rms);
}
@@ -231,7 +234,7 @@ double readionexh(FILE *fp, double *lats, double *lons, double *hgts,
/* read ionex body -----------------------------------------------------------*/
int readionexb(FILE *fp, const double *lats, const double *lons,
const double *hgts, double rb, double nexp, nav_t *nav)
const double *hgts, double rb, double nexp, nav_t *nav)
{
tec_t *p = NULL;
gtime_t time = {0, 0};
@@ -273,22 +276,23 @@ int readionexb(FILE *fp, const double *lats, const double *lons,
}
if (type == 2)
{
for (i = nav->nt-1; i >= 0; i--)
for (i = nav->nt - 1; i >= 0; i--)
{
if (fabs(timediff(time, nav->tec[i].time)) >= 1.0) continue;
p = nav->tec + i;
break;
}
}
else if (p) p->time = time;
else if (p)
p->time = time;
}
else if (strstr(label, "LAT/LON1/LON2/DLON/H") == label && p)
{
lat = str2num(buff, 2, 6);
lon[0] = str2num(buff, 8, 6);
lat = str2num(buff, 2, 6);
lon[0] = str2num(buff, 8, 6);
lon[1] = str2num(buff, 14, 6);
lon[2] = str2num(buff, 20, 6);
hgt = str2num(buff, 26, 6);
hgt = str2num(buff, 26, 6);
i = getindex(lat, p->lats);
k = getindex(hgt, p->hgts);
@@ -303,8 +307,10 @@ int readionexb(FILE *fp, const double *lats, const double *lons,
if ((x = str2num(buff, m % 16 * 5, 5)) == 9999.0) continue;
if (type == 1) p->data[index] = x * std::pow(10.0, nexp);
else p->rms[index] = (float)(x * std::pow(10.0, nexp));
if (type == 1)
p->data[index] = x * std::pow(10.0, nexp);
else
p->rms[index] = (float)(x * std::pow(10.0, nexp));
}
}
}
@@ -337,7 +343,7 @@ void combtec(nav_t *nav)
if (i > 0 && timediff(nav->tec[i].time, nav->tec[n - 1].time) == 0.0)
{
free(nav->tec[n - 1].data);
free(nav->tec[n - 1].rms );
free(nav->tec[n - 1].rms);
nav->tec[n - 1] = nav->tec[i];
continue;
}
@@ -380,7 +386,7 @@ void readtec(const char *file, nav_t *nav, int opt)
{
if (!(efiles[i] = (char *)malloc(1024)))
{
for (i--; i >= 0; i--) free(efiles[i]);
for (i--; i >= 0; i--) free(efiles[i]);
return;
}
}
@@ -423,7 +429,7 @@ void readtec(const char *file, nav_t *nav, int opt)
/* interpolate tec grid data -------------------------------------------------*/
int interptec(const tec_t *tec, int k, const double *posp, double *value,
double *rms)
double *rms)
{
double dlat, dlon, a, b, d[4] = {0}, r[4] = {0};
int i, j, n, index;
@@ -435,8 +441,10 @@ int interptec(const tec_t *tec, int k, const double *posp, double *value,
dlat = posp[0] * R2D - tec->lats[0];
dlon = posp[1] * R2D - tec->lons[0];
if (tec->lons[2] > 0.0) dlon -= floor( dlon / 360) * 360.0; /* 0 <= dlon<360 */
else dlon += floor(-dlon / 360) * 360.0; /* -360<dlon <= 0 */
if (tec->lons[2] > 0.0)
dlon -= floor(dlon / 360) * 360.0; /* 0 <= dlon<360 */
else
dlon += floor(-dlon / 360) * 360.0; /* -360<dlon <= 0 */
a = dlat / tec->lats[2];
b = dlon / tec->lons[2];
@@ -455,19 +463,41 @@ int interptec(const tec_t *tec, int k, const double *posp, double *value,
if (d[0] > 0.0 && d[1] > 0.0 && d[2] > 0.0 && d[3] > 0.0)
{
/* bilinear interpolation (inside of grid) */
*value = (1.0 - a) * (1.0 - b) * d[0] + a*(1.0 -b) * d[1] + (1.0 - a) * b * d[2] + a * b * d[3];
*rms = (1.0 - a) * (1.0 - b) * r[0] + a*(1.0 - b) * r[1] + (1.0 - a) * b * r[2] + a * b * r[3];
*value = (1.0 - a) * (1.0 - b) * d[0] + a * (1.0 - b) * d[1] + (1.0 - a) * b * d[2] + a * b * d[3];
*rms = (1.0 - a) * (1.0 - b) * r[0] + a * (1.0 - b) * r[1] + (1.0 - a) * b * r[2] + a * b * r[3];
}
/* nearest-neighbour extrapolation (outside of grid) */
else if (a <= 0.5 && b <= 0.5 && d[0] > 0.0) {*value = d[0]; *rms = r[0];}
else if (a > 0.5 && b <= 0.5 && d[1] > 0.0) {*value = d[1]; *rms = r[1];}
else if (a <= 0.5 && b > 0.5 && d[2] > 0.0) {*value = d[2]; *rms = r[2];}
else if (a > 0.5 && b > 0.5 && d[3] > 0.0) {*value = d[3]; *rms = r[3];}
else if (a <= 0.5 && b <= 0.5 && d[0] > 0.0)
{
*value = d[0];
*rms = r[0];
}
else if (a > 0.5 && b <= 0.5 && d[1] > 0.0)
{
*value = d[1];
*rms = r[1];
}
else if (a <= 0.5 && b > 0.5 && d[2] > 0.0)
{
*value = d[2];
*rms = r[2];
}
else if (a > 0.5 && b > 0.5 && d[3] > 0.0)
{
*value = d[3];
*rms = r[3];
}
else
{
i = 0;
for (n = 0; n < 4; n++) if (d[n] > 0.0) {i++; *value += d[n]; *rms += r[n];}
if(i == 0) return 0;
for (n = 0; n < 4; n++)
if (d[n] > 0.0)
{
i++;
*value += d[n];
*rms += r[n];
}
if (i == 0) return 0;
*value /= i;
*rms /= i;
}
@@ -477,31 +507,31 @@ int interptec(const tec_t *tec, int k, const double *posp, double *value,
/* ionosphere delay by tec grid data -----------------------------------------*/
int iondelay(gtime_t time, const tec_t *tec, const double *pos,
const double *azel, int opt, double *delay, double *var)
const double *azel, int opt, double *delay, double *var)
{
const double fact = 40.30E16 / FREQ1 / FREQ1; /* tecu->L1 iono (m) */
double fs, posp[3] = {0}, vtec, rms, hion, rp;
int i;
trace(3, "iondelay: time=%s pos=%.1f %.1f azel=%.1f %.1f\n", time_str(time, 0),
pos[0]*R2D, pos[1]*R2D, azel[0]*R2D, azel[1]*R2D);
pos[0] * R2D, pos[1] * R2D, azel[0] * R2D, azel[1] * R2D);
*delay = *var = 0.0;
for (i = 0;i < tec->ndata[2]; i++)
for (i = 0; i < tec->ndata[2]; i++)
{ /* for a layer */
hion = tec->hgts[0] + tec->hgts[2] * i;
/* ionospheric pierce point position */
fs = ionppp(pos, azel, tec->rb, hion, posp);
if (opt&2)
if (opt & 2)
{
/* modified single layer mapping function (M-SLM) ref [2] */
rp = tec->rb / (tec->rb + hion) * sin(0.9782 * (PI / 2.0 - azel[1]));
fs = 1.0 / sqrt(1.0 - rp * rp);
}
if (opt&1)
if (opt & 1)
{
/* earth rotation correction (sun-fixed coordinate) */
posp[1] += 2.0 * PI * timediff(time, tec->time) / 86400.0;
@@ -534,13 +564,13 @@ int iondelay(gtime_t time, const tec_t *tec, const double *pos,
* return ok with delay=0 and var=VAR_NOTEC if el<MIN_EL or h<MIN_HGT
*-----------------------------------------------------------------------------*/
int iontec(gtime_t time, const nav_t *nav, const double *pos,
const double *azel, int opt, double *delay, double *var)
const double *azel, int opt, double *delay, double *var)
{
double dels[2], vars[2], a, tt;
int i, stat[2];
trace(3, "iontec : time=%s pos=%.1f %.1f azel=%.1f %.1f\n", time_str(time, 0),
pos[0] * R2D, pos[1] * R2D, azel[0] * R2D, azel[1] * R2D);
pos[0] * R2D, pos[1] * R2D, azel[0] * R2D, azel[1] * R2D);
if (azel[1] < MIN_EL || pos[2] < MIN_HGT)
{
@@ -557,36 +587,36 @@ int iontec(gtime_t time, const nav_t *nav, const double *pos,
trace(2, "%s: tec grid out of period\n", time_str(time, 0));
return 0;
}
if ((tt = timediff(nav->tec[i].time, nav->tec[i-1].time)) == 0.0)
if ((tt = timediff(nav->tec[i].time, nav->tec[i - 1].time)) == 0.0)
{
trace(2, "tec grid time interval error\n");
return 0;
}
/* ionospheric delay by tec grid data */
stat[0] = iondelay(time, nav->tec+i-1, pos, azel, opt, dels , vars );
stat[1] = iondelay(time, nav->tec+i , pos, azel, opt, dels+1, vars+1);
stat[0] = iondelay(time, nav->tec + i - 1, pos, azel, opt, dels, vars);
stat[1] = iondelay(time, nav->tec + i, pos, azel, opt, dels + 1, vars + 1);
if (!stat[0] && !stat[1])
{
trace(2, "%s: tec grid out of area pos=%6.2f %7.2f azel=%6.1f %5.1f\n",
time_str(time, 0), pos[0] * R2D, pos[1] * R2D, azel[0] * R2D, azel[1] * R2D);
time_str(time, 0), pos[0] * R2D, pos[1] * R2D, azel[0] * R2D, azel[1] * R2D);
return 0;
}
if (stat[0] && stat[1])
{ /* linear interpolation by time */
a = timediff(time, nav->tec[i-1].time) / tt;
a = timediff(time, nav->tec[i - 1].time) / tt;
*delay = dels[0] * (1.0 - a) + dels[1] * a;
*var = vars[0] * (1.0 - a) + vars[1] * a;
*var = vars[0] * (1.0 - a) + vars[1] * a;
}
else if (stat[0])
{ /* nearest-neighbour extrapolation by time */
*delay = dels[0];
*var = vars[0];
*var = vars[0];
}
else
{
*delay = dels[1];
*var = vars[1];
*var = vars[1];
}
trace(3, "iontec : delay=%5.2f std=%5.2f\n", *delay, sqrt(*var));
return 1;

18
src/algorithms/libs/rtklib/rtklib_ionex.h
View File

@@ -62,29 +62,29 @@
#include "rtklib.h"
const double VAR_NOTEC = 30.0 * 30.0; /* variance of no tec */
const double MIN_EL = 0.0; /* min elevation angle (rad) */
const double MIN_HGT = -1000.0; /* min user height (m) */
const double VAR_NOTEC = 30.0 * 30.0; /* variance of no tec */
const double MIN_EL = 0.0; /* min elevation angle (rad) */
const double MIN_HGT = -1000.0; /* min user height (m) */
int getindex(double value, const double *range);
int nitem(const double *range);
int dataindex(int i, int j, int k, const int *ndata);
tec_t *addtec(const double *lats, const double *lons, const double *hgts,
double rb, nav_t *nav);
double rb, nav_t *nav);
void readionexdcb(FILE *fp, double *dcb, double *rms);
double readionexh(FILE *fp, double *lats, double *lons, double *hgts,
double *rb, double *nexp, double *dcb, double *rms);
double *rb, double *nexp, double *dcb, double *rms);
int readionexb(FILE *fp, const double *lats, const double *lons,
const double *hgts, double rb, double nexp, nav_t *nav);
const double *hgts, double rb, double nexp, nav_t *nav);
void combtec(nav_t *nav);
void readtec(const char *file, nav_t *nav, int opt);
int interptec(const tec_t *tec, int k, const double *posp, double *value,
double *rms);
double *rms);
int iondelay(gtime_t time, const tec_t *tec, const double *pos,
const double *azel, int opt, double *delay, double *var);
const double *azel, int opt, double *delay, double *var);
int iontec(gtime_t time, const nav_t *nav, const double *pos,
const double *azel, int opt, double *delay, double *var);
const double *azel, int opt, double *delay, double *var);
#endif /* GNSS_SDR_RTKLIB_IONEX_H_ */

219
src/algorithms/libs/rtklib/rtklib_lambda.cc
View File

@@ -56,20 +56,25 @@
/* LD factorization (Q=L'*diag(D)*L) -----------------------------------------*/
int LD(int n, const double *Q, double *L, double *D)
{
int i,j,k,info = 0;
double a,*A = mat(n,n);
int i, j, k, info = 0;
double a, *A = mat(n, n);
memcpy(A,Q,sizeof(double)*n*n);
for (i = n-1; i >= 0; i--)
memcpy(A, Q, sizeof(double) * n * n);
for (i = n - 1; i >= 0; i--)
{
if ((D[i] = A[i+i*n])<=0.0) {info = -1; break;}
if ((D[i] = A[i + i * n]) <= 0.0)
{
info = -1;
break;
}
a = sqrt(D[i]);
for (j = 0; j<=i; j++) L[i+j*n] = A[i+j*n]/a;
for (j = 0; j<=i-1; j++) for (k = 0;k<=j;k++) A[j+k*n] -= L[i+k*n]*L[i+j*n];
for (j = 0; j<=i; j++) L[i+j*n] /= L[i+i*n];
for (j = 0; j <= i; j++) L[i + j * n] = A[i + j * n] / a;
for (j = 0; j <= i - 1; j++)
for (k = 0; k <= j; k++) A[j + k * n] -= L[i + k * n] * L[i + j * n];
for (j = 0; j <= i; j++) L[i + j * n] /= L[i + i * n];
}
free(A);
if (info) fprintf(stderr,"%s : LD factorization error\n",__FILE__);
if (info) fprintf(stderr, "%s : LD factorization error\n", __FILE__);
return info;
}
@@ -77,12 +82,12 @@ int LD(int n, const double *Q, double *L, double *D)
/* integer gauss transformation ----------------------------------------------*/
void gauss(int n, double *L, double *Z, int i, int j)
{
int k,mu;
int k, mu;
if ((mu = (int)ROUND_LAMBDA(L[i+j*n]))!=0)
if ((mu = (int)ROUND_LAMBDA(L[i + j * n])) != 0)
{
for (k = i; k<n; k++) L[k+n*j] -= (double)mu*L[k+i*n];
for (k = 0; k<n; k++) Z[k+n*j] -= (double)mu*Z[k+i*n];
for (k = i; k < n; k++) L[k + n * j] -= (double)mu * L[k + i * n];
for (k = 0; k < n; k++) Z[k + n * j] -= (double)mu * Z[k + i * n];
}
}
@@ -91,116 +96,134 @@ void gauss(int n, double *L, double *Z, int i, int j)
void perm(int n, double *L, double *D, int j, double del, double *Z)
{
int k;
double eta,lam,a0,a1;
double eta, lam, a0, a1;
eta = D[j]/del;
lam = D[j+1]*L[j+1+j*n]/del;
D[j] = eta*D[j+1]; D[j+1] = del;
for (k = 0; k<=j-1; k++)
eta = D[j] / del;
lam = D[j + 1] * L[j + 1 + j * n] / del;
D[j] = eta * D[j + 1];
D[j + 1] = del;
for (k = 0; k <= j - 1; k++)
{
a0 = L[j+k*n];
a1 = L[j+1+k*n];
L[j+k*n] = -L[j+1+j*n]*a0+a1;
L[j+1+k*n] = eta*a0+lam*a1;
a0 = L[j + k * n];
a1 = L[j + 1 + k * n];
L[j + k * n] = -L[j + 1 + j * n] * a0 + a1;
L[j + 1 + k * n] = eta * a0 + lam * a1;
}
L[j+1+j*n] = lam;
for (k = j+2;k<n;k++) SWAP_LAMBDA(L[k+j*n],L[k+(j+1)*n]);
for (k = 0;k<n;k++) SWAP_LAMBDA(Z[k+j*n],Z[k+(j+1)*n]);
L[j + 1 + j * n] = lam;
for (k = j + 2; k < n; k++) SWAP_LAMBDA(L[k + j * n], L[k + (j + 1) * n]);
for (k = 0; k < n; k++) SWAP_LAMBDA(Z[k + j * n], Z[k + (j + 1) * n]);
}
/* lambda reduction (z=Z'*a, Qz=Z'*Q*Z=L'*diag(D)*L) (ref.[1]) ---------------*/
void reduction(int n, double *L, double *D, double *Z)
{
int i,j,k;
int i, j, k;
double del;
j = n-2; k = n-2;
j = n - 2;
k = n - 2;
while (j >= 0)
{
if (j<=k) for (i = j+1; i<n; i++) gauss(n,L,Z,i,j);
del = D[j]+L[j+1+j*n]*L[j+1+j*n]*D[j+1];
if (del+1E-6<D[j+1])
if (j <= k)
for (i = j + 1; i < n; i++) gauss(n, L, Z, i, j);
del = D[j] + L[j + 1 + j * n] * L[j + 1 + j * n] * D[j + 1];
if (del + 1E-6 < D[j + 1])
{ /* compared considering numerical error */
perm(n,L,D,j,del,Z);
k = j; j = n-2;
perm(n, L, D, j, del, Z);
k = j;
j = n - 2;
}
else j--;
else
j--;
}
}
/* modified lambda (mlambda) search (ref. [2]) -------------------------------*/
int search(int n, int m, const double *L, const double *D,
const double *zs, double *zn, double *s)
const double *zs, double *zn, double *s)
{
int i,j,k,c,nn = 0,imax = 0;
double newdist,maxdist = 1E99,y;
double *S = zeros(n,n),*dist = mat(n,1),*zb = mat(n,1),*z = mat(n,1),*step = mat(n,1);
int i, j, k, c, nn = 0, imax = 0;
double newdist, maxdist = 1E99, y;
double *S = zeros(n, n), *dist = mat(n, 1), *zb = mat(n, 1), *z = mat(n, 1), *step = mat(n, 1);
k = n-1; dist[k] = 0.0;
k = n - 1;
dist[k] = 0.0;
zb[k] = zs[k];
z[k] = ROUND_LAMBDA(zb[k]);
y = zb[k]-z[k];
y = zb[k] - z[k];
step[k] = SGN_LAMBDA(y);
for (c = 0; c<LOOPMAX; c++)
for (c = 0; c < LOOPMAX; c++)
{
newdist = dist[k]+y*y/D[k];
if (newdist<maxdist)
newdist = dist[k] + y * y / D[k];
if (newdist < maxdist)
{
if (k!=0)
if (k != 0)
{
dist[--k] = newdist;
for (i = 0; i<=k; i++)
S[k+i*n] = S[k+1+i*n]+(z[k+1]-zb[k+1])*L[k+1+i*n];
zb[k] = zs[k]+S[k+k*n];
z[k] = ROUND_LAMBDA(zb[k]); y = zb[k]-z[k]; step[k] = SGN_LAMBDA(y);
for (i = 0; i <= k; i++)
S[k + i * n] = S[k + 1 + i * n] + (z[k + 1] - zb[k + 1]) * L[k + 1 + i * n];
zb[k] = zs[k] + S[k + k * n];
z[k] = ROUND_LAMBDA(zb[k]);
y = zb[k] - z[k];
step[k] = SGN_LAMBDA(y);
}
else
{
if (nn<m)
if (nn < m)
{
if (nn == 0||newdist>s[imax]) imax = nn;
for (i = 0; i<n; i++) zn[i+nn*n] = z[i];
if (nn == 0 || newdist > s[imax]) imax = nn;
for (i = 0; i < n; i++) zn[i + nn * n] = z[i];
s[nn++] = newdist;
}
else
{
if (newdist<s[imax])
if (newdist < s[imax])
{
for (i = 0; i<n; i++) zn[i+imax*n] = z[i];
for (i = 0; i < n; i++) zn[i + imax * n] = z[i];
s[imax] = newdist;
for (i = imax = 0; i<m; i++) if (s[imax]<s[i]) imax = i;
for (i = imax = 0; i < m; i++)
if (s[imax] < s[i]) imax = i;
}
maxdist = s[imax];
}
z[0] += step[0]; y = zb[0]-z[0]; step[0] = -step[0]-SGN_LAMBDA(step[0]);
z[0] += step[0];
y = zb[0] - z[0];
step[0] = -step[0] - SGN_LAMBDA(step[0]);
}
}
else
{
if (k == n-1) break;
if (k == n - 1)
break;
else
{
k++;
z[k] += step[k]; y = zb[k]-z[k]; step[k] = -step[k]-SGN_LAMBDA(step[k]);
z[k] += step[k];
y = zb[k] - z[k];
step[k] = -step[k] - SGN_LAMBDA(step[k]);
}
}
}
for (i = 0; i<m-1; i++)
for (i = 0; i < m - 1; i++)
{ /* sort by s */
for (j = i+1; j<m; j++)
for (j = i + 1; j < m; j++)
{
if (s[i]<s[j]) continue;
SWAP_LAMBDA(s[i],s[j]);
for (k = 0; k<n; k++) SWAP_LAMBDA(zn[k+i*n],zn[k+j*n]);
if (s[i] < s[j]) continue;
SWAP_LAMBDA(s[i], s[j]);
for (k = 0; k < n; k++) SWAP_LAMBDA(zn[k + i * n], zn[k + j * n]);
}
}
free(S); free(dist); free(zb); free(z); free(step);
free(S);
free(dist);
free(zb);
free(z);
free(step);
if (c >= LOOPMAX)
{
fprintf(stderr,"%s : search loop count overflow\n",__FILE__);
fprintf(stderr, "%s : search loop count overflow\n", __FILE__);
return -1;
}
return 0;
@@ -220,39 +243,40 @@ int search(int n, int m, const double *L, const double *D,
* notes : matrix stored by column-major order (fortran convension)
*-----------------------------------------------------------------------------*/
int lambda(int n, int m, const double *a, const double *Q, double *F,
double *s)
double *s)
{
int info;
double *L,*D,*Z,*z,*E;
double *L, *D, *Z, *z, *E;
if (n<=0||m<=0) return -1;
L = zeros(n,n);
D = mat(n,1);
if (n <= 0 || m <= 0) return -1;
L = zeros(n, n);
D = mat(n, 1);
Z = eye(n);
z = mat(n,1);
E = mat(n,m);
z = mat(n, 1);
E = mat(n, m);
/* LD factorization */
if (!(info = LD(n,Q,L,D)))
if (!(info = LD(n, Q, L, D)))
{
/* lambda reduction */
reduction(n,L,D,Z);
matmul("TN",n,1,n,1.0,Z,a,0.0,z); /* z=Z'*a */
reduction(n, L, D, Z);
matmul("TN", n, 1, n, 1.0, Z, a, 0.0, z); /* z=Z'*a */
/* mlambda search */
if (!(info = search(n,m,L,D,z,E,s)))
if (!(info = search(n, m, L, D, z, E, s)))
{
info = solve("T",Z,E,n,m,F); /* F=Z'\E */
info = solve("T", Z, E, n, m, F); /* F=Z'\E */
}
}
free(L); free(D); free(Z); free(z); free(E);
free(L);
free(D);
free(Z);
free(z);
free(E);
return info;
}
/* lambda reduction ------------------------------------------------------------
* reduction by lambda (ref [1]) for integer least square
* args : int n I number of float parameters
@@ -262,27 +286,28 @@ int lambda(int n, int m, const double *a, const double *Q, double *F,
*-----------------------------------------------------------------------------*/
int lambda_reduction(int n, const double *Q, double *Z)
{
double *L,*D;
int i,j,info;
double *L, *D;
int i, j, info;
if (n<=0) return -1;
if (n <= 0) return -1;
L = zeros(n,n);
D = mat(n,1);
L = zeros(n, n);
D = mat(n, 1);
for (i = 0; i<n; i++) for (j = 0; j<n; j++)
{
Z[i+j*n] = i == j ? 1.0 : 0.0;
}
for (i = 0; i < n; i++)
for (j = 0; j < n; j++)
{
Z[i + j * n] = i == j ? 1.0 : 0.0;
}
/* LD factorization */
if ((info = LD(n,Q,L,D)))
if ((info = LD(n, Q, L, D)))
{
free(L);
free(D);
return info;
}
/* lambda reduction */
reduction(n,L,D,Z);
reduction(n, L, D, Z);
free(L);
free(D);
@@ -301,25 +326,25 @@ int lambda_reduction(int n, const double *Q, double *Z)
* return : status (0:ok,other:error)
*-----------------------------------------------------------------------------*/
int lambda_search(int n, int m, const double *a, const double *Q,
double *F, double *s)
double *F, double *s)
{
double *L,*D;
double *L, *D;
int info;
if (n<=0||m<=0) return -1;
if (n <= 0 || m <= 0) return -1;
L = zeros(n,n);
D = mat(n,1);
L = zeros(n, n);
D = mat(n, 1);
/* LD factorization */
if ((info = LD(n,Q,L,D)))
if ((info = LD(n, Q, L, D)))
{
free(L);
free(D);
return info;
}
/* mlambda search */
info = search(n,m,L,D,a,F,s);
info = search(n, m, L, D, a, F, s);
free(L);
free(D);

20
src/algorithms/libs/rtklib/rtklib_lambda.h
View File

@@ -64,24 +64,32 @@
#include "rtklib.h"
/* constants/macros ----------------------------------------------------------*/
const int LOOPMAX = 10000; /* maximum count of search loop */
#define SGN_LAMBDA(x) ((x)<=0.0?-1.0:1.0)
#define ROUND_LAMBDA(x) (floor((x)+0.5))
#define SWAP_LAMBDA(x,y) do {double tmp_; tmp_=x; x=y; y=tmp_;} while (0)
const int LOOPMAX = 10000; /* maximum count of search loop */
#define SGN_LAMBDA(x) ((x) <= 0.0 ? -1.0 : 1.0)
#define ROUND_LAMBDA(x) (floor((x) + 0.5))
#define SWAP_LAMBDA(x, y) \
do \
{ \
double tmp_; \
tmp_ = x; \
x = y; \
y = tmp_; \
} \
while (0)
int LD(int n, const double *Q, double *L, double *D);
void gauss(int n, double *L, double *Z, int i, int j);
void perm(int n, double *L, double *D, int j, double del, double *Z);
void reduction(int n, double *L, double *D, double *Z);
int search(int n, int m, const double *L, const double *D,
const double *zs, double *zn, double *s);
const double *zs, double *zn, double *s);
int lambda(int n, int m, const double *a, const double *Q, double *F, double *s);
int lambda_reduction(int n, const double *Q, double *Z);
int lambda_search(int n, int m, const double *a, const double *Q,
double *F, double *s);
double *F, double *s);
#endif

250
src/algorithms/libs/rtklib/rtklib_pntpos.cc
View File

@@ -81,7 +81,7 @@ double gettgd(int sat, const nav_t *nav)
/* psendorange with code bias correction -------------------------------------*/
double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
int iter, const prcopt_t *opt, double *var)
int iter, const prcopt_t *opt, double *var)
{
const double *lam = nav->lam[obs->sat - 1];
double PC, P1, P2, P1_P2, P1_C1, P2_C2, gamma_;
@@ -97,15 +97,24 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
/* L1-L2 for GPS/GLO/QZS, L1-L5 for GAL/SBS */
if (sys & (SYS_GAL | SYS_SBS)) {j = 2;}
if (sys & (SYS_GAL | SYS_SBS))
{
j = 2;
}
if (sys == SYS_GPS)
{
if(obs->code[1] != CODE_NONE) {j = 1;}
else if(obs->code[2] != CODE_NONE) {j = 2;}
if (obs->code[1] != CODE_NONE)
{
j = 1;
}
else if (obs->code[2] != CODE_NONE)
{
j = 2;
}
}
if (NFREQ<2 || lam[i] == 0.0 || lam[j] == 0.0)
if (NFREQ < 2 || lam[i] == 0.0 || lam[j] == 0.0)
{
trace(4, "prange: NFREQ<2||lam[i]==0.0||lam[j]==0.0\n");
printf("i: %d j:%d, lam[i]: %f lam[j] %f\n", i, j, lam[i], lam[j]);
@@ -118,7 +127,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
if (testsnr(0, i, azel[1], obs->SNR[i] * 0.25, &opt->snrmask))
{
trace(4, "snr mask: %s sat=%2d el=%.1f snr=%.1f\n",
time_str(obs->time, 0), obs->sat, azel[1] * R2D, obs->SNR[i] * 0.25);
time_str(obs->time, 0), obs->sat, azel[1] * R2D, obs->SNR[i] * 0.25);
return 0.0;
}
if (opt->ionoopt == IONOOPT_IFLC)
@@ -133,48 +142,63 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
gamma_ = std::pow(lam[j], 2.0) / std::pow(lam[i], 2.0); /* f1^2/f2^2 */
P1 = obs->P[i];
P2 = obs->P[j];
P1_P2 = nav->cbias[obs->sat-1][0];
P1_C1 = nav->cbias[obs->sat-1][1];
P2_C2 = nav->cbias[obs->sat-1][2];
P1_P2 = nav->cbias[obs->sat - 1][0];
P1_C1 = nav->cbias[obs->sat - 1][1];
P2_C2 = nav->cbias[obs->sat - 1][2];
/* if no P1-P2 DCB, use TGD instead */
if (P1_P2 == 0.0 && (sys & (SYS_GPS | SYS_GAL | SYS_QZS))) //CHECK!
if (P1_P2 == 0.0 && (sys & (SYS_GPS | SYS_GAL | SYS_QZS))) //CHECK!
{
P1_P2 = (1.0 - gamma_) * gettgd(obs->sat, nav);
}
if (opt->ionoopt == IONOOPT_IFLC)
{ /* dual-frequency */
if (P1 == 0.0 || P2 == 0.0) { return 0.0; }
if (obs->code[i] == CODE_L1C) { P1 += P1_C1; } /* C1->P1 */
if (obs->code[j] == CODE_L2C) { P2 += P2_C2; } /* C2->P2 */
if (P1 == 0.0 || P2 == 0.0)
{
return 0.0;
}
if (obs->code[i] == CODE_L1C)
{
P1 += P1_C1;
} /* C1->P1 */
if (obs->code[j] == CODE_L2C)
{
P2 += P2_C2;
} /* C2->P2 */
/* iono-free combination */
PC = (gamma_ * P1 - P2) / (gamma_ - 1.0);
}
else
{ /* single-frequency */
if((obs->code[i] == CODE_NONE) && (obs->code[j] == CODE_NONE)) { return 0.0; }
if ((obs->code[i] == CODE_NONE) && (obs->code[j] == CODE_NONE))
{
return 0.0;
}
else if((obs->code[i] != CODE_NONE) && (obs->code[j] == CODE_NONE))
{
else if ((obs->code[i] != CODE_NONE) && (obs->code[j] == CODE_NONE))
{
P1 += P1_C1; /* C1->P1 */
PC = P1 - P1_P2 / (1.0 - gamma_);
}
else if((obs->code[i] == CODE_NONE) && (obs->code[j] != CODE_NONE))
{
P2 += P2_C2; /* C2->P2 */
PC = P2 - gamma_ * P1_P2 / (1.0 - gamma_);
}
/* dual-frequency */
else
{
P1 += P1_C1;
P2 += P2_C2;
PC = (gamma_ * P1 - P2) / (gamma_ - 1.0);
}
}
if (opt->sateph == EPHOPT_SBAS) { PC -= P1_C1; } /* sbas clock based C1 */
}
else if ((obs->code[i] == CODE_NONE) && (obs->code[j] != CODE_NONE))
{
P2 += P2_C2; /* C2->P2 */
PC = P2 - gamma_ * P1_P2 / (1.0 - gamma_);
}
/* dual-frequency */
else
{
P1 += P1_C1;
P2 += P2_C2;
PC = (gamma_ * P1 - P2) / (gamma_ - 1.0);
}
}
if (opt->sateph == EPHOPT_SBAS)
{
PC -= P1_C1;
} /* sbas clock based C1 */
*var = std::pow(ERR_CBIAS, 2.0);
@@ -195,11 +219,11 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
* return : status(1:ok,0:error)
*-----------------------------------------------------------------------------*/
int ionocorr(gtime_t time, const nav_t *nav, int sat, const double *pos,
const double *azel, int ionoopt, double *ion, double *var)
const double *azel, int ionoopt, double *ion, double *var)
{
trace(4, "ionocorr: time=%s opt=%d sat=%2d pos=%.3f %.3f azel=%.3f %.3f\n",
time_str(time, 3), ionoopt, sat, pos[0]*R2D, pos[1]*R2D, azel[0]*R2D,
azel[1]*R2D);
time_str(time, 3), ionoopt, sat, pos[0] * R2D, pos[1] * R2D, azel[0] * R2D,
azel[1] * R2D);
/* broadcast model */
if (ionoopt == IONOOPT_BRDC)
@@ -219,7 +243,7 @@ int ionocorr(gtime_t time, const nav_t *nav, int sat, const double *pos,
return iontec(time, nav, pos, azel, 1, ion, var);
}
/* qzss broadcast model */
if (ionoopt == IONOOPT_QZS && norm_rtk(nav->ion_qzs, 8)>0.0)
if (ionoopt == IONOOPT_QZS && norm_rtk(nav->ion_qzs, 8) > 0.0)
{
*ion = ionmodel(time, nav->ion_qzs, pos, azel);
*var = std::pow(*ion * ERR_BRDCI, 2.0);
@@ -247,11 +271,11 @@ int ionocorr(gtime_t time, const nav_t *nav, int sat, const double *pos,
* return : status(1:ok,0:error)
*-----------------------------------------------------------------------------*/
int tropcorr(gtime_t time, const nav_t *nav __attribute__((unused)), const double *pos,
const double *azel, int tropopt, double *trp, double *var)
const double *azel, int tropopt, double *trp, double *var)
{
trace(4, "tropcorr: time=%s opt=%d pos=%.3f %.3f azel=%.3f %.3f\n",
time_str(time, 3), tropopt, pos[0]*R2D, pos[1]*R2D, azel[0]*R2D,
azel[1]*R2D);
time_str(time, 3), tropopt, pos[0] * R2D, pos[1] * R2D, azel[0] * R2D,
azel[1] * R2D);
/* saastamoinen model */
if (tropopt == TROPOPT_SAAS || tropopt == TROPOPT_EST || tropopt == TROPOPT_ESTG)
@@ -275,10 +299,10 @@ int tropcorr(gtime_t time, const nav_t *nav __attribute__((unused)), const doubl
/* pseudorange residuals -----------------------------------------------------*/
int rescode(int iter, const obsd_t *obs, int n, const double *rs,
const double *dts, const double *vare, const int *svh,
const nav_t *nav, const double *x, const prcopt_t *opt,
double *v, double *H, double *var, double *azel, int *vsat,
double *resp, int *ns)
const double *dts, const double *vare, const int *svh,
const nav_t *nav, const double *x, const prcopt_t *opt,
double *v, double *H, double *var, double *azel, int *vsat,
double *resp, int *ns)
{
double r, dion, dtrp, vmeas, vion, vtrp, rr[3], pos[3], dtr, e[3], P, lam_L1;
int i, j, nv = 0, sys, mask[4] = {0};
@@ -293,15 +317,15 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs,
for (i = *ns = 0; i < n && i < MAXOBS; i++)
{
vsat[i] = 0;
azel[i*2] = azel[1+i*2] = resp[i] = 0.0;
azel[i * 2] = azel[1 + i * 2] = resp[i] = 0.0;
if (!(sys = satsys(obs[i].sat, NULL))) continue;
/* reject duplicated observation data */
if (i < n - 1 && i < MAXOBS - 1 && obs[i].sat == obs[i+1].sat)
if (i < n - 1 && i < MAXOBS - 1 && obs[i].sat == obs[i + 1].sat)
{
trace(2, "duplicated observation data %s sat=%2d\n",
time_str(obs[i].time, 3), obs[i].sat);
time_str(obs[i].time, 3), obs[i].sat);
i++;
continue;
}
@@ -312,13 +336,13 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs,
continue;
}
double elaux = satazel(pos, e, azel + i * 2);
if(elaux < opt->elmin)
if (elaux < opt->elmin)
{
trace(4, "satazel error. el = %lf , elmin = %lf\n", elaux, opt->elmin);
continue;
}
/* psudorange with code bias correction */
if ((P = prange(obs+i, nav, azel+i*2, iter, opt, &vmeas)) == 0.0)
if ((P = prange(obs + i, nav, azel + i * 2, iter, opt, &vmeas)) == 0.0)
{
trace(4, "prange error\n");
continue;
@@ -332,8 +356,8 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs,
}
/* ionospheric corrections */
if (!ionocorr(obs[i].time, nav, obs[i].sat, pos, azel+i*2,
iter>0 ? opt->ionoopt : IONOOPT_BRDC, &dion, &vion))
if (!ionocorr(obs[i].time, nav, obs[i].sat, pos, azel + i * 2,
iter > 0 ? opt->ionoopt : IONOOPT_BRDC, &dion, &vion))
{
trace(4, "ionocorr error\n");
continue;
@@ -345,33 +369,49 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs,
dion *= std::pow(lam_L1 / lam_carr[0], 2.0);
}
/* tropospheric corrections */
if (!tropcorr(obs[i].time, nav, pos, azel + i*2,
if (!tropcorr(obs[i].time, nav, pos, azel + i * 2,
iter > 0 ? opt->tropopt : TROPOPT_SAAS, &dtrp, &vtrp))
{
trace(4, "tropocorr error\n");
continue;
}
/* pseudorange residual */
v[nv] = P - (r + dtr - SPEED_OF_LIGHT * dts[i*2] + dion + dtrp);
v[nv] = P - (r + dtr - SPEED_OF_LIGHT * dts[i * 2] + dion + dtrp);
/* design matrix */
for (j = 0; j < NX; j++) H[j + nv * NX] = j < 3 ? - e[j] : (j == 3 ? 1.0 : 0.0);
for (j = 0; j < NX; j++) H[j + nv * NX] = j < 3 ? -e[j] : (j == 3 ? 1.0 : 0.0);
/* time system and receiver bias offset correction */
if (sys == SYS_GLO) {v[nv] -= x[4]; H[4+nv*NX] = 1.0; mask[1] = 1;}
else if (sys == SYS_GAL) {v[nv] -= x[5]; H[5+nv*NX] = 1.0; mask[2] = 1;}
else if (sys == SYS_BDS) {v[nv] -= x[6]; H[6+nv*NX] = 1.0; mask[3] = 1;}
else mask[0] = 1;
if (sys == SYS_GLO)
{
v[nv] -= x[4];
H[4 + nv * NX] = 1.0;
mask[1] = 1;
}
else if (sys == SYS_GAL)
{
v[nv] -= x[5];
H[5 + nv * NX] = 1.0;
mask[2] = 1;
}
else if (sys == SYS_BDS)
{
v[nv] -= x[6];
H[6 + nv * NX] = 1.0;
mask[3] = 1;
}
else
mask[0] = 1;
vsat[i] = 1;
resp[i] = v[nv];
(*ns)++;
/* error variance */
var[nv++] = varerr(opt, azel[1+i*2], sys) + vare[i] + vmeas + vion + vtrp;
var[nv++] = varerr(opt, azel[1 + i * 2], sys) + vare[i] + vmeas + vion + vtrp;
trace(4, "sat=%2d azel=%5.1f %4.1f res=%7.3f sig=%5.3f\n", obs[i].sat,
azel[i*2] * R2D, azel[1+i*2] * R2D, resp[i], sqrt(var[nv-1]));
azel[i * 2] * R2D, azel[1 + i * 2] * R2D, resp[i], sqrt(var[nv - 1]));
}
/* constraint to avoid rank-deficient */
for (i = 0; i < 4; i++)
@@ -387,10 +427,10 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs,
/* validate solution ---------------------------------------------------------*/
int valsol(const double *azel, const int *vsat, int n,
const prcopt_t *opt, const double *v, int nv, int nx,
char *msg)
const prcopt_t *opt, const double *v, int nv, int nx,
char *msg)
{
double azels[MAXOBS*2] = {0};
double azels[MAXOBS * 2] = {0};
double dop[4], vv;
int i, ns;
@@ -398,17 +438,17 @@ int valsol(const double *azel, const int *vsat, int n,
/* chi-square validation of residuals */
vv = dot(v, v, nv);
if (nv > nx && vv > chisqr[nv-nx-1])
if (nv > nx && vv > chisqr[nv - nx - 1])
{
sprintf(msg, "chi-square error nv=%d vv=%.1f cs=%.1f", nv, vv, chisqr[nv-nx-1]);
sprintf(msg, "chi-square error nv=%d vv=%.1f cs=%.1f", nv, vv, chisqr[nv - nx - 1]);
return 0;
}
/* large gdop check */
for (i = ns = 0; i < n; i++)
{
if (!vsat[i]) continue;
azels[ ns*2] = azel[ i*2];
azels[1+ns*2] = azel[1+i*2];
azels[ns * 2] = azel[i * 2];
azels[1 + ns * 2] = azel[1 + i * 2];
ns++;
}
dops(ns, azels, opt->elmin, dop);
@@ -423,11 +463,11 @@ int valsol(const double *azel, const int *vsat, int n,
/* estimate receiver position ------------------------------------------------*/
int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
const double *vare, const int *svh, const nav_t *nav,
const prcopt_t *opt, sol_t *sol, double *azel, int *vsat,
double *resp, char *msg)
const double *vare, const int *svh, const nav_t *nav,
const prcopt_t *opt, sol_t *sol, double *azel, int *vsat,
double *resp, char *msg)
{
double x[NX] = {0}, dx[NX], Q[NX*NX], *v, *H, *var, sig;
double x[NX] = {0}, dx[NX], Q[NX * NX], *v, *H, *var, sig;
int i, j, k, info, stat, nv, ns;
trace(3, "estpos : n=%d\n", n);
@@ -449,7 +489,7 @@ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
break;
}
/* weight by variance */
for (j = 0;j < nv; j++)
for (j = 0; j < nv; j++)
{
sig = sqrt(var[j]);
v[j] /= sig;
@@ -473,9 +513,9 @@ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
sol->dtr[3] = x[6] / SPEED_OF_LIGHT; /* bds-gps time offset (s) */
for (j = 0; j < 6; j++) sol->rr[j] = j < 3 ? x[j] : 0.0;
for (j = 0; j < 3; j++) sol->qr[j] = (float)Q[j + j * NX];
sol->qr[3] = (float)Q[1]; /* cov xy */
sol->qr[3] = (float)Q[1]; /* cov xy */
sol->qr[4] = (float)Q[2 + NX]; /* cov yz */
sol->qr[5] = (float)Q[2]; /* cov zx */
sol->qr[5] = (float)Q[2]; /* cov zx */
sol->ns = (unsigned char)ns;
sol->age = sol->ratio = 0.0;
@@ -502,13 +542,13 @@ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
/* raim fde (failure detection and exclution) -------------------------------*/
int raim_fde(const obsd_t *obs, int n, const double *rs,
const double *dts, const double *vare, const int *svh,
const nav_t *nav, const prcopt_t *opt, sol_t *sol,
double *azel, int *vsat, double *resp, char *msg)
int raim_fde(const obsd_t *obs, int n, const double *rs,
const double *dts, const double *vare, const int *svh,
const nav_t *nav, const prcopt_t *opt, sol_t *sol,
double *azel, int *vsat, double *resp, char *msg)
{
obsd_t *obs_e;
sol_t sol_e = { {0, 0}, {}, {}, {}, '0', '0', '0', 0.0, 0.0, 0.0};
sol_t sol_e = {{0, 0}, {}, {}, {}, '0', '0', '0', 0.0, 0.0, 0.0};
char tstr[32], name[16], msg_e[128];
double *rs_e, *dts_e, *vare_e, *azel_e, *resp_e, rms_e, rms = 100.0;
int i, j, k, nvsat, stat = 0, *svh_e, *vsat_e, sat = 0;
@@ -537,7 +577,7 @@ int raim_fde(const obsd_t *obs, int n, const double *rs,
svh_e[k++] = svh[j];
}
/* estimate receiver position without a satellite */
if (!estpos(obs_e, n-1, rs_e, dts_e, vare_e, svh_e, nav, opt, &sol_e, azel_e,
if (!estpos(obs_e, n - 1, rs_e, dts_e, vare_e, svh_e, nav, opt, &sol_e, azel_e,
vsat_e, resp_e, msg_e))
{
trace(3, "raim_fde: exsat=%2d (%s)\n", obs[i].sat, msg);
@@ -552,7 +592,7 @@ int raim_fde(const obsd_t *obs, int n, const double *rs,
if (nvsat < 5)
{
trace(3, "raim_fde: exsat=%2d lack of satellites nvsat=%2d\n",
obs[i].sat, nvsat);
obs[i].sat, nvsat);
continue;
}
rms_e = sqrt(rms_e / nvsat);
@@ -578,7 +618,8 @@ int raim_fde(const obsd_t *obs, int n, const double *rs,
}
if (stat)
{
time2str(obs[0].time, tstr, 2); satno2id(sat, name);
time2str(obs[0].time, tstr, 2);
satno2id(sat, name);
trace(2, "%s: %s excluded by raim\n", tstr + 11, name);
}
free(obs_e);
@@ -596,8 +637,8 @@ int raim_fde(const obsd_t *obs, int n, const double *rs,
/* doppler residuals ---------------------------------------------------------*/
int resdop(const obsd_t *obs, int n, const double *rs, const double *dts,
const nav_t *nav, const double *rr, const double *x,
const double *azel, const int *vsat, double *v, double *H)
const nav_t *nav, const double *rr, const double *x,
const double *azel, const int *vsat, double *v, double *H)
{
double lam, rate, pos[3], E[9], a[3], e[3], vs[3], cosel;
int i, j, nv = 0;
@@ -609,31 +650,30 @@ int resdop(const obsd_t *obs, int n, const double *rs, const double *dts,
for (i = 0; i < n && i < MAXOBS; i++)
{
lam = nav->lam[obs[i].sat-1][0];
lam = nav->lam[obs[i].sat - 1][0];
if (obs[i].D[0] == 0.0 || lam == 0.0 || !vsat[i] || norm_rtk(rs + 3 + i * 6, 3) <= 0.0)
{
continue;
}
/* line-of-sight vector in ecef */
cosel = cos(azel[1+i*2]);
a[0] = sin(azel[i*2]) * cosel;
a[1] = cos(azel[i*2]) * cosel;
a[2] = sin(azel[1+i*2]);
cosel = cos(azel[1 + i * 2]);
a[0] = sin(azel[i * 2]) * cosel;
a[1] = cos(azel[i * 2]) * cosel;
a[2] = sin(azel[1 + i * 2]);
matmul("TN", 3, 1, 3, 1.0, E, a, 0.0, e);
/* satellite velocity relative to receiver in ecef */
for (j = 0; j < 3; j++) vs[j] = rs[j+3+i*6] - x[j];
for (j = 0; j < 3; j++) vs[j] = rs[j + 3 + i * 6] - x[j];
/* range rate with earth rotation correction */
rate = dot(vs, e, 3) + DEFAULT_OMEGA_EARTH_DOT / SPEED_OF_LIGHT * (rs[4 + i * 6] * rr[0] + rs[1 + i * 6] * x[0]-
rs[3 + i * 6] * rr[1] - rs[i * 6] * x[1]);
rate = dot(vs, e, 3) + DEFAULT_OMEGA_EARTH_DOT / SPEED_OF_LIGHT * (rs[4 + i * 6] * rr[0] + rs[1 + i * 6] * x[0] - rs[3 + i * 6] * rr[1] - rs[i * 6] * x[1]);
/* doppler residual */
v[nv] =- lam * obs[i].D[0] - (rate + x[3] - SPEED_OF_LIGHT * dts[1 + i *2]);
v[nv] = -lam * obs[i].D[0] - (rate + x[3] - SPEED_OF_LIGHT * dts[1 + i * 2]);
/* design matrix */
for (j = 0; j < 4; j++) H[j + nv * 4] = j < 3 ? - e[j] : 1.0;
for (j = 0; j < 4; j++) H[j + nv * 4] = j < 3 ? -e[j] : 1.0;
nv++;
}
@@ -643,8 +683,8 @@ int resdop(const obsd_t *obs, int n, const double *rs, const double *dts,
/* estimate receiver velocity ------------------------------------------------*/
void estvel(const obsd_t *obs, int n, const double *rs, const double *dts,
const nav_t *nav, const prcopt_t *opt __attribute__((unused)), sol_t *sol,
const double *azel, const int *vsat)
const nav_t *nav, const prcopt_t *opt __attribute__((unused)), sol_t *sol,
const double *azel, const int *vsat)
{
double x[4] = {0}, dx[4], Q[16], *v, *H;
int i, j, nv;
@@ -668,7 +708,7 @@ void estvel(const obsd_t *obs, int n, const double *rs, const double *dts,
if (norm_rtk(dx, 4) < 1e-6)
{
for (i = 0; i < 3; i++) sol->rr[i+3] = x[i];
for (i = 0; i < 3; i++) sol->rr[i + 3] = x[i];
break;
}
}
@@ -694,8 +734,8 @@ void estvel(const obsd_t *obs, int n, const double *rs, const double *dts,
* and receiver bias)
*-----------------------------------------------------------------------------*/
int pntpos(const obsd_t *obs, int n, const nav_t *nav,
const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat,
char *msg)
const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat,
char *msg)
{
prcopt_t opt_ = *opt;
double *rs, *dts, *var, *azel_, *resp;
@@ -703,7 +743,11 @@ int pntpos(const obsd_t *obs, int n, const nav_t *nav,
sol->stat = SOLQ_NONE;
if (n <= 0) {strcpy(msg, "no observation data"); return 0;}
if (n <= 0)
{
strcpy(msg, "no observation data");
return 0;
}
trace(3, "pntpos : tobs=%s n=%d\n", time_str(obs[0].time, 3), n);
@@ -753,12 +797,12 @@ int pntpos(const obsd_t *obs, int n, const nav_t *nav,
}
for (i = 0; i < n; i++)
{
ssat[obs[i].sat-1].azel[0] = azel_[ i*2];
ssat[obs[i].sat-1].azel[1] = azel_[1+i*2];
ssat[obs[i].sat-1].snr[0] = obs[i].SNR[0];
ssat[obs[i].sat - 1].azel[0] = azel_[i * 2];
ssat[obs[i].sat - 1].azel[1] = azel_[1 + i * 2];
ssat[obs[i].sat - 1].snr[0] = obs[i].SNR[0];
if (!vsat[i]) continue;
ssat[obs[i].sat-1].vs = 1;
ssat[obs[i].sat-1].resp[0] = resp[i];
ssat[obs[i].sat - 1].vs = 1;
ssat[obs[i].sat - 1].resp[0] = resp[i];
}
}
free(rs);

50
src/algorithms/libs/rtklib/rtklib_pntpos.h
View File

@@ -57,10 +57,10 @@
#include "rtklib_rtkcmn.h"
/* constants -----------------------------------------------------------------*/
const int NX = 4 + 3; //!< # of estimated parameters
const int MAXITR = 10; //!< max number of iteration for point pos
const double ERR_ION = 5.0; //!< ionospheric delay std (m)
const double ERR_TROP = 3.0; //!< tropspheric delay std (m)
const int NX = 4 + 3; //!< # of estimated parameters
const int MAXITR = 10; //!< max number of iteration for point pos
const double ERR_ION = 5.0; //!< ionospheric delay std (m)
const double ERR_TROP = 3.0; //!< tropspheric delay std (m)
/* pseudorange measurement error variance ------------------------------------*/
@@ -71,7 +71,7 @@ double gettgd(int sat, const nav_t *nav);
/* psendorange with code bias correction -------------------------------------*/
double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
int iter, const prcopt_t *opt, double *var);
int iter, const prcopt_t *opt, double *var);
/* ionospheric correction ------------------------------------------------------
* compute ionospheric correction
@@ -86,7 +86,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
* return : status(1:ok,0:error)
*-----------------------------------------------------------------------------*/
int ionocorr(gtime_t time, const nav_t *nav, int sat, const double *pos,
const double *azel, int ionoopt, double *ion, double *var);
const double *azel, int ionoopt, double *ion, double *var);
/* tropospheric correction -----------------------------------------------------
* compute tropospheric correction
* args : gtime_t time I time
@@ -99,41 +99,41 @@ int ionocorr(gtime_t time, const nav_t *nav, int sat, const double *pos,
* return : status(1:ok,0:error)
*-----------------------------------------------------------------------------*/
int tropcorr(gtime_t time, const nav_t *nav, const double *pos,
const double *azel, int tropopt, double *trp, double *var);
const double *azel, int tropopt, double *trp, double *var);
/* pseudorange residuals -----------------------------------------------------*/
int rescode(int iter, const obsd_t *obs, int n, const double *rs,
const double *dts, const double *vare, const int *svh,
const nav_t *nav, const double *x, const prcopt_t *opt,
double *v, double *H, double *var, double *azel, int *vsat,
double *resp, int *ns);
const double *dts, const double *vare, const int *svh,
const nav_t *nav, const double *x, const prcopt_t *opt,
double *v, double *H, double *var, double *azel, int *vsat,
double *resp, int *ns);
/* validate solution ---------------------------------------------------------*/
int valsol(const double *azel, const int *vsat, int n,
const prcopt_t *opt, const double *v, int nv, int nx,
char *msg);
const prcopt_t *opt, const double *v, int nv, int nx,
char *msg);
/* estimate receiver position ------------------------------------------------*/
int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
const double *vare, const int *svh, const nav_t *nav,
const prcopt_t *opt, sol_t *sol, double *azel, int *vsat,
double *resp, char *msg);
const double *vare, const int *svh, const nav_t *nav,
const prcopt_t *opt, sol_t *sol, double *azel, int *vsat,
double *resp, char *msg);
/* raim fde (failure detection and exclution) -------------------------------*/
int raim_fde(const obsd_t *obs, int n, const double *rs,
const double *dts, const double *vare, const int *svh,
const nav_t *nav, const prcopt_t *opt, sol_t *sol,
double *azel, int *vsat, double *resp, char *msg);
const double *dts, const double *vare, const int *svh,
const nav_t *nav, const prcopt_t *opt, sol_t *sol,
double *azel, int *vsat, double *resp, char *msg);
/* doppler residuals ---------------------------------------------------------*/
int resdop(const obsd_t *obs, int n, const double *rs, const double *dts,
const nav_t *nav, const double *rr, const double *x,
const double *azel, const int *vsat, double *v, double *H);
const nav_t *nav, const double *rr, const double *x,
const double *azel, const int *vsat, double *v, double *H);
/* estimate receiver velocity ------------------------------------------------*/
void estvel(const obsd_t *obs, int n, const double *rs, const double *dts,
const nav_t *nav, const prcopt_t *opt, sol_t *sol,
const double *azel, const int *vsat);
const nav_t *nav, const prcopt_t *opt, sol_t *sol,
const double *azel, const int *vsat);
/*!
* \brief single-point positioning
@@ -153,7 +153,7 @@ void estvel(const obsd_t *obs, int n, const double *rs, const double *dts,
* and receiver bias)
*/
int pntpos(const obsd_t *obs, int n, const nav_t *nav,
const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat,
char *msg);
const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat,
char *msg);
#endif /* GNSS_SDR_RTKLIB_PNTPOS_H_ */

959
src/algorithms/libs/rtklib/rtklib_ppp.cc
View File

File diff suppressed because it is too large Load Diff

71
src/algorithms/libs/rtklib/rtklib_ppp.h
View File

@@ -57,23 +57,37 @@
#include "rtklib.h"
#define MIN_PPP(x,y) ((x)<=(y)?(x):(y))
#define ROUND_PPP(x) (int)floor((x)+0.5)
#define MIN_PPP(x, y) ((x) <= (y) ? (x) : (y))
#define ROUND_PPP(x) (int)floor((x) + 0.5)
#define SWAP_I(x,y) do {int _z=x; x=y; y=_z;} while (0)
#define SWAP_D(x,y) do {double _z=x; x=y; y=_z;} while (0)
#define SWAP_I(x, y) \
do \
{ \
int _z = x; \
x = y; \
y = _z; \
} \
while (0)
#define SWAP_D(x, y) \
do \
{ \
double _z = x; \
x = y; \
y = _z; \
} \
while (0)
const double MIN_ARC_GAP = 300.0; /* min arc gap (s) */
const double CONST_AMB = 0.001; /* constraint to fixed ambiguity */
const double THRES_RES = 0.3; /* threashold of residuals test (m) */
const double MIN_ARC_GAP = 300.0; /* min arc gap (s) */
const double CONST_AMB = 0.001; /* constraint to fixed ambiguity */
const double THRES_RES = 0.3; /* threashold of residuals test (m) */
const double LOG_PI = 1.14472988584940017; /* log(pi) */
const double SQRT2 = 1.41421356237309510; /* sqrt(2) */
const double SQRT2 = 1.41421356237309510; /* sqrt(2) */
const double VAR_POS_PPP = std::pow(100.0, 2.0); /* init variance receiver position (m^2) */
const double VAR_CLK = std::pow(100.0, 2.0); /* init variance receiver clock (m^2) */
const double VAR_ZTD = std::pow( 0.3, 2.0); /* init variance ztd (m^2) */
const double VAR_GRA_PPP = std::pow(0.001, 2.0); /* init variance gradient (m^2) */
const double VAR_BIAS = std::pow(100.0, 2.0); /* init variance phase-bias (m^2) */
const double VAR_POS_PPP = std::pow(100.0, 2.0); /* init variance receiver position (m^2) */
const double VAR_CLK = std::pow(100.0, 2.0); /* init variance receiver clock (m^2) */
const double VAR_ZTD = std::pow(0.3, 2.0); /* init variance ztd (m^2) */
const double VAR_GRA_PPP = std::pow(0.001, 2.0); /* init variance gradient (m^2) */
const double VAR_BIAS = std::pow(100.0, 2.0); /* init variance phase-bias (m^2) */
const double VAR_IONO_OFF = std::pow(10.0, 2.0); /* variance of iono-model-off */
@@ -112,15 +126,14 @@ int fix_amb_ILS(rtk_t *rtk, int *sat1, int *sat2, int *NW, int n);
int pppamb(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav, const double *azel);
/* functions originally included in RTKLIB/src/ppp.c v2.4.2 */
void pppoutsolstat(rtk_t *rtk, int level, FILE *fp);
void tide_pl(const double *eu, const double *rp, double GMp, const double *pos, double *dr);
void tide_solid(const double *rsun, const double *rmoon,
const double *pos, const double *E, double gmst, int opt,
double *dr);
const double *pos, const double *E, double gmst, int opt,
double *dr);
void tide_oload(gtime_t tut, const double *odisp, double *denu);
@@ -129,7 +142,7 @@ void iers_mean_pole(gtime_t tut, double *xp_bar, double *yp_bar);
void tide_pole(gtime_t tut, const double *pos, const double *erpv, double *denu);
void tidedisp(gtime_t tutc, const double *rr, int opt, const erp_t *erp,
const double *odisp, double *dr);
const double *odisp, double *dr);
void testeclipse(const obsd_t *obs, int n, const nav_t *nav, double *rs);
@@ -138,19 +151,19 @@ double varerr(int sat, int sys, double el, int type, const prcopt_t *opt);
void initx(rtk_t *rtk, double xi, double var, int i);
int ifmeas(const obsd_t *obs, const nav_t *nav, const double *azel,
const prcopt_t *opt, const double *dantr, const double *dants,
double phw, double *meas, double *var);
const prcopt_t *opt, const double *dantr, const double *dants,
double phw, double *meas, double *var);
double gettgd_ppp(int sat, const nav_t *nav);
int corr_ion(gtime_t time, const nav_t *nav, int sat, const double *pos,
const double *azel, int ionoopt, double *ion, double *var,
int *brk);
const double *azel, int ionoopt, double *ion, double *var,
int *brk);
int corrmeas(const obsd_t *obs, const nav_t *nav, const double *pos,
const double *azel, const prcopt_t *opt,
const double *dantr, const double *dants, double phw,
double *meas, double *var, int *brk);
const double *azel, const prcopt_t *opt,
const double *dantr, const double *dants, double phw,
double *meas, double *var, int *brk);
double gfmeas(const obsd_t *obs, const nav_t *nav);
@@ -171,13 +184,13 @@ void udstate_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav);
void satantpcv(const double *rs, const double *rr, const pcv_t *pcv, double *dant);
double prectrop(gtime_t time, const double *pos, const double *azel,
const prcopt_t *opt, const double *x, double *dtdx,
double *var);
const prcopt_t *opt, const double *x, double *dtdx,
double *var);
int res_ppp(int iter, const obsd_t *obs, int n, const double *rs,
const double *dts, const double *vare, const int *svh,
const nav_t *nav, const double *x, rtk_t *rtk, double *v,
double *H, double *R, double *azel);
const double *dts, const double *vare, const int *svh,
const nav_t *nav, const double *x, rtk_t *rtk, double *v,
double *H, double *R, double *azel);
int pppnx(const prcopt_t *opt);

258
src/algorithms/libs/rtklib/rtklib_preceph.cc
View File

@@ -78,14 +78,14 @@ int code2sys(char code)
/* read sp3 header -----------------------------------------------------------*/
int readsp3h(FILE *fp, gtime_t *time, char *type, int *sats,
double *bfact, char *tsys)
double *bfact, char *tsys)
{
int i, j, k = 0, ns = 0, sys, prn;
char buff[1024];
trace(3, "readsp3h:\n");
for (i = 0;i<22;i++)
for (i = 0; i < 22; i++)
{
if (!fgets(buff, sizeof(buff), fp)) break;
@@ -94,26 +94,27 @@ int readsp3h(FILE *fp, gtime_t *time, char *type, int *sats,
*type = buff[2];
if (str2time(buff, 3, 28, time)) return 0;
}
else if (2<=i && i<=6)
else if (2 <= i && i <= 6)
{
if (i == 2)
{
ns = (int)str2num(buff, 4, 2);
}
for (j = 0;j<17 && k<ns;j++)
for (j = 0; j < 17 && k < ns; j++)
{
sys = code2sys(buff[9+3*j]);
prn = (int)str2num(buff, 10+3*j, 2);
if (k<MAXSAT) sats[k++] = satno(sys, prn);
sys = code2sys(buff[9 + 3 * j]);
prn = (int)str2num(buff, 10 + 3 * j, 2);
if (k < MAXSAT) sats[k++] = satno(sys, prn);
}
}
else if (i == 12)
{
strncpy(tsys, buff+9, 3); tsys[3] = '\0';
strncpy(tsys, buff + 9, 3);
tsys[3] = '\0';
}
else if (i == 14)
{
bfact[0] = str2num(buff, 3, 10);
bfact[0] = str2num(buff, 3, 10);
bfact[1] = str2num(buff, 14, 12);
}
}
@@ -129,7 +130,7 @@ int addpeph(nav_t *nav, peph_t *peph)
if (nav->ne >= nav->nemax)
{
nav->nemax += 256;
if (!(nav_peph = (peph_t *)realloc(nav->peph, sizeof(peph_t)*nav->nemax)))
if (!(nav_peph = (peph_t *)realloc(nav->peph, sizeof(peph_t) * nav->nemax)))
{
trace(1, "readsp3b malloc error n=%d\n", nav->nemax);
free(nav->peph);
@@ -146,12 +147,12 @@ int addpeph(nav_t *nav, peph_t *peph)
/* read sp3 body -------------------------------------------------------------*/
void readsp3b(FILE *fp, char type, int *sats __attribute__((unused)), int ns, double *bfact,
char *tsys, int index, int opt, nav_t *nav)
char *tsys, int index, int opt, nav_t *nav)
{
peph_t peph;
gtime_t time;
double val, std, base;
int i, j, sat, sys, prn, n = ns*(type == 'P' ? 1 : 2), pred_o, pred_c, v;
int i, j, sat, sys, prn, n = ns * (type == 'P' ? 1 : 2), pred_o, pred_c, v;
char buff[1024];
trace(3, "readsp3b: type=%c ns=%d index=%d opt=%d\n", type, ns, index, opt);
@@ -166,7 +167,7 @@ void readsp3b(FILE *fp, char type, int *sats __attribute__((unused)), int ns, do
continue;
}
if (!strcmp(tsys, "UTC")) time = utc2gpst(time); /* utc->gpst */
peph.time = time;
peph.time = time;
peph.index = index;
for (i = 0; i < MAXSAT; i++)
@@ -184,14 +185,16 @@ void readsp3b(FILE *fp, char type, int *sats __attribute__((unused)), int ns, do
peph.vco[i][j] = 0.0f;
}
}
for (i = pred_o = pred_c = v = 0;i<n && fgets(buff, sizeof(buff), fp);i++)
for (i = pred_o = pred_c = v = 0; i < n && fgets(buff, sizeof(buff), fp); i++)
{
if (strlen(buff) < 4 || (buff[0] != 'P' && buff[0] != 'V')) continue;
sys = buff[1] == ' '?SYS_GPS:code2sys(buff[1]);
sys = buff[1] == ' ' ? SYS_GPS : code2sys(buff[1]);
prn = (int)str2num(buff, 2, 2);
if (sys == SYS_SBS) prn += 100;
else if (sys == SYS_QZS) prn += 192; /* extension to sp3-c */
if (sys == SYS_SBS)
prn += 100;
else if (sys == SYS_QZS)
prn += 192; /* extension to sp3-c */
if (!(sat = satno(sys, prn))) continue;
@@ -200,38 +203,40 @@ void readsp3b(FILE *fp, char type, int *sats __attribute__((unused)), int ns, do
pred_c = strlen(buff) >= 76 && buff[75] == 'P';
pred_o = strlen(buff) >= 80 && buff[79] == 'P';
}
for (j = 0;j<4;j++)
for (j = 0; j < 4; j++)
{
/* read option for predicted value */
if (j< 3 && (opt&1) && pred_o) continue;
if (j< 3 && (opt&2) && !pred_o) continue;
if (j == 3 && (opt&1) && pred_c) continue;
if (j == 3 && (opt&2) && !pred_c) continue;
if (j < 3 && (opt & 1) && pred_o) continue;
if (j < 3 && (opt & 2) && !pred_o) continue;
if (j == 3 && (opt & 1) && pred_c) continue;
if (j == 3 && (opt & 2) && !pred_c) continue;
val = str2num(buff, 4 + j * 14, 14);
val = str2num(buff, 4 + j * 14, 14);
std = str2num(buff, 61 + j * 3, j < 3 ? 2 : 3);
if (buff[0] == 'P') { /* position */
if (buff[0] == 'P')
{ /* position */
if (val != 0.0 && fabs(val - 999999.999999) >= 1e-6)
{
peph.pos[sat-1][j] = val*(j < 3 ? 1000.0 : 1e-6);
peph.pos[sat - 1][j] = val * (j < 3 ? 1000.0 : 1e-6);
v = 1; /* valid epoch */
}
if ((base = bfact[j < 3 ? 0 : 1]) > 0.0 && std > 0.0)
{
peph.std[sat-1][j] = (float)(std::pow(base, std)*(j < 3 ? 1e-3 : 1e-12));
peph.std[sat - 1][j] = (float)(std::pow(base, std) * (j < 3 ? 1e-3 : 1e-12));
}
}
else if (v) { /* velocity */
}
else if (v)
{ /* velocity */
if (val != 0.0 && fabs(val - 999999.999999) >= 1e-6)
{
peph.vel[sat-1][j] = val*(j < 3 ? 0.1 : 1e-10);
peph.vel[sat - 1][j] = val * (j < 3 ? 0.1 : 1e-10);
}
if ((base = bfact[j < 3 ? 0 : 1]) > 0.0 && std > 0.0)
{
peph.vst[sat-1][j] = (float)(std::pow(base, std)*(j < 3 ? 1e-7 : 1e-16));
peph.vst[sat - 1][j] = (float)(std::pow(base, std) * (j < 3 ? 1e-7 : 1e-16));
}
}
}
}
}
if (v)
@@ -243,7 +248,7 @@ void readsp3b(FILE *fp, char type, int *sats __attribute__((unused)), int ns, do
/* compare precise ephemeris -------------------------------------------------*/
int cmppeph(const void *p1, const void *p2)
int cmppeph(const void *p1, const void *p2)
{
peph_t *q1 = (peph_t *)p1, *q2 = (peph_t *)p2;
double tt = timediff(q1->time, q2->time);
@@ -252,7 +257,7 @@ int cmppeph(const void *p1, const void *p2)
/* combine precise ephemeris -------------------------------------------------*/
void combpeph(nav_t *nav, int opt)
void combpeph(nav_t *nav, int opt)
{
int i, j, k, m;
@@ -260,7 +265,7 @@ void combpeph(nav_t *nav, int opt)
qsort(nav->peph, nav->ne, sizeof(peph_t), cmppeph);
if (opt&4) return;
if (opt & 4) return;
for (i = 0, j = 1; j < nav->ne; j++)
{
@@ -269,15 +274,16 @@ void combpeph(nav_t *nav, int opt)
for (k = 0; k < MAXSAT; k++)
{
if (norm_rtk(nav->peph[j].pos[k], 4) <= 0.0) continue;
for (m = 0;m < 4; m++) nav->peph[i].pos[k][m] = nav->peph[j].pos[k][m];
for (m = 0;m < 4; m++) nav->peph[i].std[k][m] = nav->peph[j].std[k][m];
for (m = 0;m < 4; m++) nav->peph[i].vel[k][m] = nav->peph[j].vel[k][m];
for (m = 0;m < 4; m++) nav->peph[i].vst[k][m] = nav->peph[j].vst[k][m];
for (m = 0; m < 4; m++) nav->peph[i].pos[k][m] = nav->peph[j].pos[k][m];
for (m = 0; m < 4; m++) nav->peph[i].std[k][m] = nav->peph[j].std[k][m];
for (m = 0; m < 4; m++) nav->peph[i].vel[k][m] = nav->peph[j].vel[k][m];
for (m = 0; m < 4; m++) nav->peph[i].vst[k][m] = nav->peph[j].vst[k][m];
}
}
else if (++i < j) nav->peph[i] = nav->peph[j];
else if (++i < j)
nav->peph[i] = nav->peph[j];
}
nav->ne = i+1;
nav->ne = i + 1;
trace(4, "combpeph: ne=%d\n", nav->ne);
}
@@ -323,7 +329,7 @@ void readsp3(const char *file, nav_t *nav, int opt)
if (!(ext = strrchr(efiles[i], '.'))) continue;
if (!strstr(ext + 1, "sp3") && !strstr(ext + 1, ".SP3") &&
!strstr(ext + 1, "eph") && !strstr(ext + 1, ".EPH")) continue;
!strstr(ext + 1, "eph") && !strstr(ext + 1, ".EPH")) continue;
if (!(fp = fopen(efiles[i], "r")))
{
@@ -355,8 +361,8 @@ void readsp3(const char *file, nav_t *nav, int opt)
*-----------------------------------------------------------------------------*/
int readsap(const char *file, gtime_t time, nav_t *nav)
{
pcvs_t pcvs = {0, 0, (pcv_t*){ 0 } };
pcv_t pcv0 = { 0, {}, {}, {0,0}, {0,0}, {{},{}}, {{},{}} }, *pcv;
pcvs_t pcvs = {0, 0, (pcv_t *){0}};
pcv_t pcv0 = {0, {}, {}, {0, 0}, {0, 0}, {{}, {}}, {{}, {}}}, *pcv;
int i;
trace(3, "readsap : file=%s time=%s\n", file, time_str(time, 0));
@@ -390,9 +396,12 @@ int readdcbf(const char *file, nav_t *nav, const sta_t *sta)
}
while (fgets(buff, sizeof(buff), fp))
{
if (strstr(buff, "DIFFERENTIAL (P1-P2) CODE BIASES")) type = 1;
else if (strstr(buff, "DIFFERENTIAL (P1-C1) CODE BIASES")) type = 2;
else if (strstr(buff, "DIFFERENTIAL (P2-C2) CODE BIASES")) type = 3;
if (strstr(buff, "DIFFERENTIAL (P1-P2) CODE BIASES"))
type = 1;
else if (strstr(buff, "DIFFERENTIAL (P1-C1) CODE BIASES"))
type = 2;
else if (strstr(buff, "DIFFERENTIAL (P2-C2) CODE BIASES"))
type = 3;
if (!type || sscanf(buff, "%s %s", str1, str2) < 1) continue;
@@ -407,12 +416,12 @@ int readdcbf(const char *file, nav_t *nav, const sta_t *sta)
if (i < MAXRCV)
{
j = !strcmp(str1, "G") ? 0 : 1;
nav->rbias[i][j][type-1] = cbias * 1e-9 * SPEED_OF_LIGHT; /* ns -> m */
nav->rbias[i][j][type - 1] = cbias * 1e-9 * SPEED_OF_LIGHT; /* ns -> m */
}
}
else if ((sat = satid2no(str1)))
{ /* satellite dcb */
nav->cbias[sat-1][type-1] = cbias * 1e-9 * SPEED_OF_LIGHT; /* ns -> m */
{ /* satellite dcb */
nav->cbias[sat - 1][type - 1] = cbias * 1e-9 * SPEED_OF_LIGHT; /* ns -> m */
}
}
fclose(fp);
@@ -437,10 +446,11 @@ int readdcb(const char *file, nav_t *nav, const sta_t *sta)
trace(3, "readdcb : file=%s\n", file);
for (i = 0;i < MAXSAT; i++) for (j = 0; j < 3; j++)
{
nav->cbias[i][j] = 0.0;
}
for (i = 0; i < MAXSAT; i++)
for (j = 0; j < 3; j++)
{
nav->cbias[i][j] = 0.0;
}
for (i = 0; i < MAXEXFILE; i++)
{
if (!(efiles[i] = (char *)malloc(1024)))
@@ -455,7 +465,7 @@ int readdcb(const char *file, nav_t *nav, const sta_t *sta)
{
readdcbf(efiles[i], nav, sta);
}
for (i = 0; i < MAXEXFILE;i ++) free(efiles[i]);
for (i = 0; i < MAXEXFILE; i++) free(efiles[i]);
return 1;
}
@@ -463,24 +473,24 @@ int readdcb(const char *file, nav_t *nav, const sta_t *sta)
/* add satellite fcb ---------------------------------------------------------*/
int addfcb(nav_t *nav, gtime_t ts, gtime_t te, int sat,
const double *bias, const double *std)
const double *bias, const double *std)
{
fcbd_t *nav_fcb;
int i, j;
if (nav->nf > 0 && fabs(timediff(ts, nav->fcb[nav->nf-1].ts)) <= 1e-3)
if (nav->nf > 0 && fabs(timediff(ts, nav->fcb[nav->nf - 1].ts)) <= 1e-3)
{
for (i = 0; i < 3; i++)
{
nav->fcb[nav->nf-1].bias[sat-1][i] = bias[i];
nav->fcb[nav->nf-1].std [sat-1][i] = std [i];
nav->fcb[nav->nf - 1].bias[sat - 1][i] = bias[i];
nav->fcb[nav->nf - 1].std[sat - 1][i] = std[i];
}
return 1;
}
if (nav->nf >= nav->nfmax)
{
nav->nfmax = nav->nfmax <= 0 ? 2048 : nav->nfmax * 2;
if (!(nav_fcb = (fcbd_t *)realloc(nav->fcb, sizeof(fcbd_t)*nav->nfmax)))
if (!(nav_fcb = (fcbd_t *)realloc(nav->fcb, sizeof(fcbd_t) * nav->nfmax)))
{
free(nav->fcb);
nav->nf = nav->nfmax = 0;
@@ -488,16 +498,17 @@ int addfcb(nav_t *nav, gtime_t ts, gtime_t te, int sat,
}
nav->fcb = nav_fcb;
}
for (i = 0; i < MAXSAT; i++) for (j = 0; j < 3; j++)
{
nav->fcb[nav->nf].bias[i][j] = nav->fcb[nav->nf].std[i][j] = 0.0;
}
for (i = 0; i < MAXSAT; i++)
for (j = 0; j < 3; j++)
{
nav->fcb[nav->nf].bias[i][j] = nav->fcb[nav->nf].std[i][j] = 0.0;
}
for (i = 0; i < 3; i++)
{
nav->fcb[nav->nf].bias[sat-1][i] = bias[i];
nav->fcb[nav->nf].std [sat-1][i] = std [i];
nav->fcb[nav->nf].bias[sat - 1][i] = bias[i];
nav->fcb[nav->nf].std[sat - 1][i] = std[i];
}
nav->fcb[nav->nf ].ts = ts;
nav->fcb[nav->nf].ts = ts;
nav->fcb[nav->nf++].te = te;
return 1;
}
@@ -522,10 +533,12 @@ int readfcbf(const char *file, nav_t *nav)
while (fgets(buff, sizeof(buff), fp))
{
if ((p = strchr(buff, '#'))) *p = '\0';
if (sscanf(buff, "%lf/%lf/%lf %lf:%lf:%lf %lf/%lf/%lf %lf:%lf:%lf %s"
"%lf %lf %lf %lf %lf %lf", ep1, ep1+1, ep1+2, ep1+3, ep1+4, ep1+5,
ep2, ep2+1, ep2+2, ep2+3, ep2+4, ep2+5, str, bias, std, bias+1, std+1,
bias+2, std+2) < 17) continue;
if (sscanf(buff,
"%lf/%lf/%lf %lf:%lf:%lf %lf/%lf/%lf %lf:%lf:%lf %s"
"%lf %lf %lf %lf %lf %lf",
ep1, ep1 + 1, ep1 + 2, ep1 + 3, ep1 + 4, ep1 + 5,
ep2, ep2 + 1, ep2 + 2, ep2 + 3, ep2 + 4, ep2 + 5, str, bias, std, bias + 1, std + 1,
bias + 2, std + 2) < 17) continue;
if (!(sat = satid2no(str))) continue;
ts = epoch2time(ep1);
te = epoch2time(ep2);
@@ -596,7 +609,7 @@ double interppol(const double *x, double *y, int n)
{
for (i = 0; i < n - j; i++)
{
y[i] = (x[i+j] * y[i] - x[i] * y[i+1]) / (x[i+j] - x[i]);
y[i] = (x[i + j] * y[i] - x[i] * y[i + 1]) / (x[i + j] - x[i]);
}
}
return y[0];
@@ -605,9 +618,9 @@ double interppol(const double *x, double *y, int n)
/* satellite position by precise ephemeris -----------------------------------*/
int pephpos(gtime_t time, int sat, const nav_t *nav, double *rs,
double *dts, double *vare, double *varc)
double *dts, double *vare, double *varc)
{
double t[NMAX+1], p[3][NMAX+1], c[2], *pos, std = 0.0, s[3], sinl, cosl;
double t[NMAX + 1], p[3][NMAX + 1], c[2], *pos, std = 0.0, s[3], sinl, cosl;
int i, j, k, index;
trace(4, "pephpos : time=%s sat=%2d\n", time_str(time, 3), sat);
@@ -615,8 +628,8 @@ int pephpos(gtime_t time, int sat, const nav_t *nav, double *rs,
rs[0] = rs[1] = rs[2] = dts[0] = 0.0;
if (nav->ne < NMAX + 1 ||
timediff(time, nav->peph[0].time) < -MAXDTE ||
timediff(time, nav->peph[nav->ne-1].time) > MAXDTE)
timediff(time, nav->peph[0].time) < -MAXDTE ||
timediff(time, nav->peph[nav->ne - 1].time) > MAXDTE)
{
trace(3, "no prec ephem %s sat=%2d\n", time_str(time, 0), sat);
return 0;
@@ -625,26 +638,32 @@ int pephpos(gtime_t time, int sat, const nav_t *nav, double *rs,
for (i = 0, j = nav->ne - 1; i < j;)
{
k = (i + j) / 2;
if (timediff(nav->peph[k].time, time) < 0.0) i = k + 1; else j = k;
if (timediff(nav->peph[k].time, time) < 0.0)
i = k + 1;
else
j = k;
}
index = i <= 0 ? 0 : i-1;
index = i <= 0 ? 0 : i - 1;
/* polynomial interpolation for orbit */
i = index - (NMAX + 1) / 2;
if (i < 0) i = 0; else if (i + NMAX >= nav->ne) i = nav->ne - NMAX - 1;
if (i < 0)
i = 0;
else if (i + NMAX >= nav->ne)
i = nav->ne - NMAX - 1;
for (j = 0; j <= NMAX; j++)
{
t[j] = timediff(nav->peph[i+j].time, time);
if (norm_rtk(nav->peph[i+j].pos[sat-1], 3) <= 0.0)
t[j] = timediff(nav->peph[i + j].time, time);
if (norm_rtk(nav->peph[i + j].pos[sat - 1], 3) <= 0.0)
{
trace(3, "prec ephem outage %s sat=%2d\n", time_str(time, 0), sat);
return 0;
}
}
for (j = 0;j<=NMAX;j++)
for (j = 0; j <= NMAX; j++)
{
pos = nav->peph[i+j].pos[sat-1];
pos = nav->peph[i + j].pos[sat - 1];
#if 0
p[0][j] = pos[0];
p[1][j] = pos[1];
@@ -663,39 +682,41 @@ int pephpos(gtime_t time, int sat, const nav_t *nav, double *rs,
}
if (vare)
{
for (i = 0; i < 3; i++) s[i] = nav->peph[index].std[sat-1][i];
for (i = 0; i < 3; i++) s[i] = nav->peph[index].std[sat - 1][i];
std = norm_rtk(s, 3);
/* extrapolation error for orbit */
if (t[0 ] > 0.0) std += EXTERR_EPH * std::pow(t[0 ], 2.0) / 2.0;
else if (t[NMAX] < 0.0) std += EXTERR_EPH * std::pow(t[NMAX], 2.0) / 2.0;
if (t[0] > 0.0)
std += EXTERR_EPH * std::pow(t[0], 2.0) / 2.0;
else if (t[NMAX] < 0.0)
std += EXTERR_EPH * std::pow(t[NMAX], 2.0) / 2.0;
*vare = std::pow(std, 2.0);
}
/* linear interpolation for clock */
t[0] = timediff(time, nav->peph[index ].time);
t[1] = timediff(time, nav->peph[index+1].time);
c[0] = nav->peph[index ].pos[sat-1][3];
c[1] = nav->peph[index+1].pos[sat-1][3];
t[0] = timediff(time, nav->peph[index].time);
t[1] = timediff(time, nav->peph[index + 1].time);
c[0] = nav->peph[index].pos[sat - 1][3];
c[1] = nav->peph[index + 1].pos[sat - 1][3];
if (t[0] <= 0.0)
{
if ((dts[0] = c[0]) != 0.0)
{
std = nav->peph[index].std[sat-1][3] * SPEED_OF_LIGHT - EXTERR_CLK * t[0];
std = nav->peph[index].std[sat - 1][3] * SPEED_OF_LIGHT - EXTERR_CLK * t[0];
}
}
else if (t[1] >= 0.0)
{
if ((dts[0] = c[1]) != 0.0)
{
std = nav->peph[index+1].std[sat-1][3] * SPEED_OF_LIGHT + EXTERR_CLK * t[1];
std = nav->peph[index + 1].std[sat - 1][3] * SPEED_OF_LIGHT + EXTERR_CLK * t[1];
}
}
else if (c[0] != 0.0 && c[1] != 0.0)
{
dts[0] = (c[1] * t[0] - c[0] * t[1]) / (t[0] - t[1]);
i = t[0] < -t[1] ? 0: 1;
std = nav->peph[index+i].std[sat-1][3] + EXTERR_CLK * fabs(t[i]);
i = t[0] < -t[1] ? 0 : 1;
std = nav->peph[index + i].std[sat - 1][3] + EXTERR_CLK * fabs(t[i]);
}
else
{
@@ -708,7 +729,7 @@ int pephpos(gtime_t time, int sat, const nav_t *nav, double *rs,
/* satellite clock by precise clock ------------------------------------------*/
int pephclk(gtime_t time, int sat, const nav_t *nav, double *dts,
double *varc)
double *varc)
{
double t[2], c[2], std;
int i, j, k, index;
@@ -716,8 +737,8 @@ int pephclk(gtime_t time, int sat, const nav_t *nav, double *dts,
trace(4, "pephclk : time=%s sat=%2d\n", time_str(time, 3), sat);
if (nav->nc < 2 ||
timediff(time, nav->pclk[0].time) < -MAXDTE ||
timediff(time, nav->pclk[nav->nc-1].time) > MAXDTE)
timediff(time, nav->pclk[0].time) < -MAXDTE ||
timediff(time, nav->pclk[nav->nc - 1].time) > MAXDTE)
{
trace(3, "no prec clock %s sat=%2d\n", time_str(time, 0), sat);
return 1;
@@ -726,31 +747,34 @@ int pephclk(gtime_t time, int sat, const nav_t *nav, double *dts,
for (i = 0, j = nav->nc - 1; i < j;)
{
k = (i + j) / 2;
if (timediff(nav->pclk[k].time, time) < 0.0) i = k + 1; else j = k;
if (timediff(nav->pclk[k].time, time) < 0.0)
i = k + 1;
else
j = k;
}
index = i<=0?0:i-1;
index = i <= 0 ? 0 : i - 1;
/* linear interpolation for clock */
t[0] = timediff(time, nav->pclk[index ].time);
t[1] = timediff(time, nav->pclk[index+1].time);
c[0] = nav->pclk[index ].clk[sat-1][0];
c[1] = nav->pclk[index+1].clk[sat-1][0];
t[0] = timediff(time, nav->pclk[index].time);
t[1] = timediff(time, nav->pclk[index + 1].time);
c[0] = nav->pclk[index].clk[sat - 1][0];
c[1] = nav->pclk[index + 1].clk[sat - 1][0];
if (t[0] <= 0.0)
{
if ((dts[0] = c[0]) == 0.0) return 0;
std = nav->pclk[index].std[sat-1][0] * SPEED_OF_LIGHT - EXTERR_CLK * t[0];
std = nav->pclk[index].std[sat - 1][0] * SPEED_OF_LIGHT - EXTERR_CLK * t[0];
}
else if (t[1] >= 0.0)
{
if ((dts[0] = c[1]) == 0.0) return 0;
std = nav->pclk[index+1].std[sat-1][0] * SPEED_OF_LIGHT + EXTERR_CLK * t[1];
std = nav->pclk[index + 1].std[sat - 1][0] * SPEED_OF_LIGHT + EXTERR_CLK * t[1];
}
else if (c[0] != 0.0 && c[1] != 0.0)
{
dts[0] = (c[1]*t[0]-c[0]*t[1]) / (t[0] - t[1]);
dts[0] = (c[1] * t[0] - c[0] * t[1]) / (t[0] - t[1]);
i = t[0] < -t[1] ? 0 : 1;
std = nav->pclk[index+i].std[sat-1][0] * SPEED_OF_LIGHT + EXTERR_CLK * fabs(t[i]);
std = nav->pclk[index + i].std[sat - 1][0] * SPEED_OF_LIGHT + EXTERR_CLK * fabs(t[i]);
}
else
{
@@ -774,10 +798,10 @@ int pephclk(gtime_t time, int sat, const nav_t *nav, double *dts,
* return : none
*-----------------------------------------------------------------------------*/
void satantoff(gtime_t time, const double *rs, int sat, const nav_t *nav,
double *dant)
double *dant)
{
const double *lam = nav->lam[sat-1];
const pcv_t *pcv = nav->pcvs+sat-1;
const double *lam = nav->lam[sat - 1];
const pcv_t *pcv = nav->pcvs + sat - 1;
double ex[3], ey[3], ez[3], es[3], r[3], rsun[3], gmst, erpv[5] = {};
double gamma, C1, C2, dant1, dant2;
int i, j = 0, k = 1;
@@ -790,7 +814,7 @@ void satantoff(gtime_t time, const double *rs, int sat, const nav_t *nav,
/* unit vectors of satellite fixed coordinates */
for (i = 0; i < 3; i++) r[i] = -rs[i];
if (!normv3(r, ez)) return;
for (i = 0; i < 3; i++) r[i] = rsun[i]-rs[i];
for (i = 0; i < 3; i++) r[i] = rsun[i] - rs[i];
if (!normv3(r, es)) return;
cross3(ez, es, r);
if (!normv3(r, ey)) return;
@@ -833,7 +857,7 @@ void satantoff(gtime_t time, const double *rs, int sat, const nav_t *nav,
* if precise clocks are not set, clocks in sp3 are used instead
*-----------------------------------------------------------------------------*/
int peph2pos(gtime_t time, int sat, const nav_t *nav, int opt,
double *rs, double *dts, double *var)
double *rs, double *dts, double *var)
{
double rss[3], rst[3], dtss[1], dtst[1], dant[3] = {}, vare = 0.0, varc = 0.0, tt = 1e-3;
int i;
@@ -844,26 +868,26 @@ int peph2pos(gtime_t time, int sat, const nav_t *nav, int opt,
/* satellite position and clock bias */
if (!pephpos(time, sat, nav, rss, dtss, &vare, &varc) ||
!pephclk(time, sat, nav, dtss, &varc)) return 0;
!pephclk(time, sat, nav, dtss, &varc)) return 0;
time = timeadd(time, tt);
if (!pephpos(time, sat, nav, rst, dtst, NULL, NULL) ||
!pephclk(time, sat, nav, dtst, NULL)) return 0;
!pephclk(time, sat, nav, dtst, NULL)) return 0;
/* satellite antenna offset correction */
if (opt)
{
satantoff(time, rss, sat, nav, dant);
}
for (i = 0;i<3;i++)
for (i = 0; i < 3; i++)
{
rs[i ] = rss[i] + dant[i];
rs[i+3] = (rst[i] - rss[i]) / tt;
rs[i] = rss[i] + dant[i];
rs[i + 3] = (rst[i] - rss[i]) / tt;
}
/* relativistic effect correction */
if (dtss[0] != 0.0)
{
dts[0] = dtss[0] - 2.0 * dot(rs, rs+3, 3) / SPEED_OF_LIGHT / SPEED_OF_LIGHT;
dts[0] = dtss[0] - 2.0 * dot(rs, rs + 3, 3) / SPEED_OF_LIGHT / SPEED_OF_LIGHT;
dts[1] = (dtst[0] - dtss[0]) / tt;
}
else

26
src/algorithms/libs/rtklib/rtklib_preceph.h
View File

@@ -66,17 +66,17 @@
#include "rtklib.h"
const int NMAX = 10; /* order of polynomial interpolation */
const double MAXDTE = 900.0; /* max time difference to ephem time (s) */
const double EXTERR_CLK = 1e-3; /* extrapolation error for clock (m/s) */
const double EXTERR_EPH = 5e-7; /* extrapolation error for ephem (m/s^2) */
const int NMAX = 10; /* order of polynomial interpolation */
const double MAXDTE = 900.0; /* max time difference to ephem time (s) */
const double EXTERR_CLK = 1e-3; /* extrapolation error for clock (m/s) */
const double EXTERR_EPH = 5e-7; /* extrapolation error for ephem (m/s^2) */
int code2sys(char code);
int readsp3h(FILE *fp, gtime_t *time, char *type, int *sats,
double *bfact, char *tsys);
double *bfact, char *tsys);
int addpeph(nav_t *nav, peph_t *peph);
void readsp3b(FILE *fp, char type, int *sats, int ns, double *bfact,
char *tsys, int index, int opt, nav_t *nav);
char *tsys, int index, int opt, nav_t *nav);
int cmppeph(const void *p1, const void *p2);
void combpeph(nav_t *nav, int opt);
void readsp3(const char *file, nav_t *nav, int opt);
@@ -84,24 +84,24 @@ int readsap(const char *file, gtime_t time, nav_t *nav);
int readdcbf(const char *file, nav_t *nav, const sta_t *sta);
int readdcb(const char *file, nav_t *nav, const sta_t *sta);
int addfcb(nav_t *nav, gtime_t ts, gtime_t te, int sat,
const double *bias, const double *std);
const double *bias, const double *std);
int readfcbf(const char *file, nav_t *nav);
int readdcb(const char *file, nav_t *nav, const sta_t *sta);
int addfcb(nav_t *nav, gtime_t ts, gtime_t te, int sat,
const double *bias, const double *std);
const double *bias, const double *std);
int readfcbf(const char *file, nav_t *nav);
int cmpfcb(const void *p1, const void *p2);
int readfcb(const char *file, nav_t *nav);
double interppol(const double *x, double *y, int n);
int pephpos(gtime_t time, int sat, const nav_t *nav, double *rs,
double *dts, double *vare, double *varc);
double *dts, double *vare, double *varc);
int pephclk(gtime_t time, int sat, const nav_t *nav, double *dts,
double *varc);
double *varc);
void satantoff(gtime_t time, const double *rs, int sat, const nav_t *nav,
double *dant);
double *dant);
int peph2pos(gtime_t time, int sat, const nav_t *nav, int opt,
double *rs, double *dts, double *var);
double *rs, double *dts, double *var);
#endif // GNSS_SDR_RTKLIB_PRECEPH_H_
#endif // GNSS_SDR_RTKLIB_PRECEPH_H_

93
src/algorithms/libs/rtklib/rtklib_rtcm.cc
View File

@@ -67,12 +67,12 @@ int init_rtcm(rtcm_t *rtcm)
{
gtime_t time0 = {0, 0.0};
obsd_t data0 = {{0, 0.0}, 0, 0, {0}, {0}, {0}, {0.0}, {0.0}, {0.0}};
eph_t eph0 = {0, -1, -1, 0, 0, 0, 0, 0, {0,0.0}, {0,0.0}, {0,0.0},
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, 0.0, 0.0};
geph_t geph0 = {0, -1, 0, 0, 0, 0, {0,0.0}, {0,0.0}, {0.0}, {0.0}, {0.0},
0.0, 0.0, 0.0};
ssr_t ssr0 = { {{0, 0.0}}, {0.0}, {0}, 0, 0, 0, 0, {0.0}, {0.0}, {0.0}, 0.0, {0.0}, {0.0}, {0.0}, 0.0, 0.0, '0' };
eph_t eph0 = {0, -1, -1, 0, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0, 0.0},
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, 0.0, 0.0};
geph_t geph0 = {0, -1, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0.0}, {0.0}, {0.0},
0.0, 0.0, 0.0};
ssr_t ssr0 = {{{0, 0.0}}, {0.0}, {0}, 0, 0, 0, 0, {0.0}, {0.0}, {0.0}, 0.0, {0.0}, {0.0}, {0.0}, 0.0, 0.0, '0'};
int i, j;
trace(3, "init_rtcm:\n");
@@ -83,38 +83,39 @@ int init_rtcm(rtcm_t *rtcm)
rtcm->sta.antdes[0] = rtcm->sta.antsno[0] = '\0';
rtcm->sta.rectype[0] = rtcm->sta.recver[0] = rtcm->sta.recsno[0] = '\0';
rtcm->sta.antsetup = rtcm->sta.itrf = rtcm->sta.deltype = 0;
for (i = 0;i < 3;i++)
for (i = 0; i < 3; i++)
{
rtcm->sta.pos[i] = rtcm->sta.del[i] = 0.0;
}
rtcm->sta.hgt = 0.0;
rtcm->dgps = NULL;
for (i = 0;i < MAXSAT;i++)
for (i = 0; i < MAXSAT; i++)
{
rtcm->ssr[i] = ssr0;
}
rtcm->msg[0] = rtcm->msgtype[0] = rtcm->opt[0] = '\0';
for (i = 0;i < 6;i++) rtcm->msmtype[i][0] = '\0';
for (i = 0; i < 6; i++) rtcm->msmtype[i][0] = '\0';
rtcm->obsflag = rtcm->ephsat = 0;
for (i = 0;i < MAXSAT;i++) for (j = 0;j < NFREQ+NEXOBS;j++)
{
rtcm->cp[i][j] = 0.0;
rtcm->lock[i][j] = rtcm->loss[i][j] = 0;
rtcm->lltime[i][j] = time0;
}
for (i = 0; i < MAXSAT; i++)
for (j = 0; j < NFREQ + NEXOBS; j++)
{
rtcm->cp[i][j] = 0.0;
rtcm->lock[i][j] = rtcm->loss[i][j] = 0;
rtcm->lltime[i][j] = time0;
}
rtcm->nbyte = rtcm->nbit = rtcm->len = 0;
rtcm->word = 0;
for (i = 0;i < 100;i++) rtcm->nmsg2[i] = 0;
for (i = 0;i < 300;i++) rtcm->nmsg3[i] = 0;
for (i = 0; i < 100; i++) rtcm->nmsg2[i] = 0;
for (i = 0; i < 300; i++) rtcm->nmsg3[i] = 0;
rtcm->obs.data = NULL;
rtcm->nav.eph = NULL;
rtcm->nav.eph = NULL;
rtcm->nav.geph = NULL;
/* reallocate memory for observation and ephemris buffer */
if (!(rtcm->obs.data = (obsd_t *)malloc(sizeof(obsd_t)*MAXOBS)) ||
!(rtcm->nav.eph = (eph_t *)malloc(sizeof(eph_t )*MAXSAT)) ||
!(rtcm->nav.geph = (geph_t *)malloc(sizeof(geph_t)*MAXPRNGLO)))
if (!(rtcm->obs.data = (obsd_t *)malloc(sizeof(obsd_t) * MAXOBS)) ||
!(rtcm->nav.eph = (eph_t *)malloc(sizeof(eph_t) * MAXSAT)) ||
!(rtcm->nav.geph = (geph_t *)malloc(sizeof(geph_t) * MAXPRNGLO)))
{
free_rtcm(rtcm);
return 0;
@@ -122,9 +123,9 @@ int init_rtcm(rtcm_t *rtcm)
rtcm->obs.n = 0;
rtcm->nav.n = MAXSAT;
rtcm->nav.ng = MAXPRNGLO;
for (i = 0;i < MAXOBS ;i++) rtcm->obs.data[i] = data0;
for (i = 0;i < MAXSAT ;i++) rtcm->nav.eph [i] = eph0;
for (i = 0;i < MAXPRNGLO;i++) rtcm->nav.geph[i] = geph0;
for (i = 0; i < MAXOBS; i++) rtcm->obs.data[i] = data0;
for (i = 0; i < MAXSAT; i++) rtcm->nav.eph[i] = eph0;
for (i = 0; i < MAXPRNGLO; i++) rtcm->nav.geph[i] = geph0;
return 1;
}
@@ -139,9 +140,15 @@ void free_rtcm(rtcm_t *rtcm)
trace(3, "free_rtcm:\n");
/* free memory for observation and ephemeris buffer */
free(rtcm->obs.data); rtcm->obs.data = NULL; rtcm->obs.n = 0;
free(rtcm->nav.eph ); rtcm->nav.eph = NULL; rtcm->nav.n = 0;
free(rtcm->nav.geph); rtcm->nav.geph = NULL; rtcm->nav.ng = 0;
free(rtcm->obs.data);
rtcm->obs.data = NULL;
rtcm->obs.n = 0;
free(rtcm->nav.eph);
rtcm->nav.eph = NULL;
rtcm->nav.n = 0;
free(rtcm->nav.geph);
rtcm->nav.geph = NULL;
rtcm->nav.ng = 0;
}
@@ -175,32 +182,38 @@ int input_rtcm2(rtcm_t *rtcm, unsigned char data)
/* synchronize frame */
if (rtcm->nbyte == 0)
{
preamb = (unsigned char)(rtcm->word>>22);
preamb = (unsigned char)(rtcm->word >> 22);
if (rtcm->word & 0x40000000) preamb ^= 0xFF; /* decode preamble */
if (preamb != RTCM2PREAMB) continue;
/* check parity */
if (!decode_word(rtcm->word, rtcm->buff)) continue;
rtcm->nbyte = 3; rtcm->nbit = 0;
rtcm->nbyte = 3;
rtcm->nbit = 0;
continue;
}
if (++rtcm->nbit < 30) continue; else rtcm->nbit = 0;
if (++rtcm->nbit < 30)
continue;
else
rtcm->nbit = 0;
/* check parity */
if (!decode_word(rtcm->word, rtcm->buff+rtcm->nbyte))
if (!decode_word(rtcm->word, rtcm->buff + rtcm->nbyte))
{
trace(2, "rtcm2 partity error: i=%d word=%08x\n", i, rtcm->word);
rtcm->nbyte = 0; rtcm->word &= 0x3;
rtcm->nbyte = 0;
rtcm->word &= 0x3;
continue;
}
rtcm->nbyte += 3;
if (rtcm->nbyte == 6) rtcm->len = (rtcm->buff[5]>>3)*3+6;
if (rtcm->nbyte == 6) rtcm->len = (rtcm->buff[5] >> 3) * 3 + 6;
if (rtcm->nbyte < rtcm->len) continue;
rtcm->nbyte = 0; rtcm->word &= 0x3;
rtcm->nbyte = 0;
rtcm->word &= 0x3;
/* decode rtcm2 message */
return decode_rtcm2(rtcm);
}
}
return 0;
}
@@ -285,13 +298,13 @@ int input_rtcm3(rtcm_t *rtcm, unsigned char data)
if (rtcm->nbyte == 3)
{
rtcm->len = getbitu(rtcm->buff, 14, 10)+3; /* length without parity */
rtcm->len = getbitu(rtcm->buff, 14, 10) + 3; /* length without parity */
}
if (rtcm->nbyte < 3 || rtcm->nbyte < rtcm->len+3) return 0;
if (rtcm->nbyte < 3 || rtcm->nbyte < rtcm->len + 3) return 0;
rtcm->nbyte = 0;
/* check parity */
if (rtk_crc24q(rtcm->buff, rtcm->len) != getbitu(rtcm->buff, rtcm->len*8, 24))
if (rtk_crc24q(rtcm->buff, rtcm->len) != getbitu(rtcm->buff, rtcm->len * 8, 24))
{
trace(2, "rtcm3 parity error: len=%d\n", rtcm->len);
return 0;
@@ -314,7 +327,7 @@ int input_rtcm2f(rtcm_t *rtcm, FILE *fp)
trace(4, "input_rtcm2f: data=%02x\n", data);
for (i = 0;i < 4096;i++)
for (i = 0; i < 4096; i++)
{
if ((data = fgetc(fp)) == EOF) return -2;
if ((ret = input_rtcm2(rtcm, (unsigned char)data))) return ret;
@@ -336,7 +349,7 @@ int input_rtcm3f(rtcm_t *rtcm, FILE *fp)
trace(4, "input_rtcm3f: data=%02x\n", data);
for (i = 0;i < 4096;i++)
for (i = 0; i < 4096; i++)
{
if ((data = fgetc(fp)) == EOF) return -2;
if ((ret = input_rtcm3(rtcm, (unsigned char)data))) return ret;

4
src/algorithms/libs/rtklib/rtklib_rtcm.h
View File

@@ -59,8 +59,8 @@
#include "rtklib_rtcm2.h"
#include "rtklib_rtcm3.h"
#define RTCM2PREAMB 0x66 /* rtcm ver.2 frame preamble */
#define RTCM3PREAMB 0xD3 /* rtcm ver.3 frame preamble */
#define RTCM2PREAMB 0x66 /* rtcm ver.2 frame preamble */
#define RTCM3PREAMB 0xD3 /* rtcm ver.3 frame preamble */
int init_rtcm(rtcm_t *rtcm);

381
src/algorithms/libs/rtklib/rtklib_rtcm2.cc
View File

@@ -52,7 +52,6 @@
*----------------------------------------------------------------------------*/
#include "rtklib_rtcm2.h"
#include "rtklib_rtkcmn.h"
@@ -66,11 +65,13 @@ void adjhour(rtcm_t *rtcm, double zcnt)
/* if no time, get cpu time */
if (rtcm->time.time == 0) rtcm->time = utc2gpst(timeget());
tow = time2gpst(rtcm->time, &week);
hour = floor(tow/3600.0);
sec = tow-hour*3600.0;
if (zcnt<sec-1800.0) zcnt += 3600.0;
else if (zcnt>sec+1800.0) zcnt-=3600.0;
rtcm->time = gpst2time(week, hour*3600+zcnt);
hour = floor(tow / 3600.0);
sec = tow - hour * 3600.0;
if (zcnt < sec - 1800.0)
zcnt += 3600.0;
else if (zcnt > sec + 1800.0)
zcnt -= 3600.0;
rtcm->time = gpst2time(week, hour * 3600 + zcnt);
}
@@ -79,7 +80,7 @@ int obsindex(obs_t *obs, gtime_t time, int sat)
{
int i, j;
for (i = 0; i<obs->n; i++)
for (i = 0; i < obs->n; i++)
{
if (obs->data[i].sat == sat) return i; /* field already exists */
}
@@ -88,7 +89,7 @@ int obsindex(obs_t *obs, gtime_t time, int sat)
/* add new field */
obs->data[i].time = time;
obs->data[i].sat = sat;
for (j = 0; j<NFREQ; j++)
for (j = 0; j < NFREQ; j++)
{
obs->data[i].L[j] = obs->data[i].P[j] = 0.0;
obs->data[i].D[j] = 0.0;
@@ -107,16 +108,22 @@ int decode_type1(rtcm_t *rtcm)
trace(4, "decode_type1: len=%d\n", rtcm->len);
while (i+40 <= rtcm->len*8)
while (i + 40 <= rtcm->len * 8)
{
fact = getbitu(rtcm->buff, i, 1); i += 1;
udre = getbitu(rtcm->buff, i, 2); i += 2;
prn = getbitu(rtcm->buff, i, 5); i += 5;
prc = getbits(rtcm->buff, i, 16); i += 16;
rrc = getbits(rtcm->buff, i, 8); i += 8;
iod = getbits(rtcm->buff, i, 8); i += 8;
fact = getbitu(rtcm->buff, i, 1);
i += 1;
udre = getbitu(rtcm->buff, i, 2);
i += 2;
prn = getbitu(rtcm->buff, i, 5);
i += 5;
prc = getbits(rtcm->buff, i, 16);
i += 16;
rrc = getbits(rtcm->buff, i, 8);
i += 8;
iod = getbits(rtcm->buff, i, 8);
i += 8;
if (prn == 0) prn = 32;
if (prc == 0x80000000||rrc == 0xFFFF8000)
if (prc == 0x80000000 || rrc == 0xFFFF8000)
{
trace(2, "rtcm2 1 prc/rrc indicates satellite problem: prn=%d\n", prn);
continue;
@@ -124,11 +131,11 @@ int decode_type1(rtcm_t *rtcm)
if (rtcm->dgps)
{
sat = satno(SYS_GPS, prn);
rtcm->dgps[sat-1].t0 = rtcm->time;
rtcm->dgps[sat-1].prc = prc*(fact?0.32:0.02);
rtcm->dgps[sat-1].rrc = rrc*(fact?0.032:0.002);
rtcm->dgps[sat-1].iod = iod;
rtcm->dgps[sat-1].udre = udre;
rtcm->dgps[sat - 1].t0 = rtcm->time;
rtcm->dgps[sat - 1].prc = prc * (fact ? 0.32 : 0.02);
rtcm->dgps[sat - 1].rrc = rrc * (fact ? 0.032 : 0.002);
rtcm->dgps[sat - 1].iod = iod;
rtcm->dgps[sat - 1].udre = udre;
}
}
return 7;
@@ -142,11 +149,13 @@ int decode_type3(rtcm_t *rtcm)
trace(4, "decode_type3: len=%d\n", rtcm->len);
if (i+96 <= rtcm->len*8)
if (i + 96 <= rtcm->len * 8)
{
rtcm->sta.pos[0] = getbits(rtcm->buff, i, 32)*0.01; i += 32;
rtcm->sta.pos[1] = getbits(rtcm->buff, i, 32)*0.01; i += 32;
rtcm->sta.pos[2] = getbits(rtcm->buff, i, 32)*0.01;
rtcm->sta.pos[0] = getbits(rtcm->buff, i, 32) * 0.01;
i += 32;
rtcm->sta.pos[1] = getbits(rtcm->buff, i, 32) * 0.01;
i += 32;
rtcm->sta.pos[2] = getbits(rtcm->buff, i, 32) * 0.01;
}
else
{
@@ -166,10 +175,12 @@ int decode_type14(rtcm_t *rtcm)
trace(4, "decode_type14: len=%d\n", rtcm->len);
zcnt = getbitu(rtcm->buff, 24, 13);
if (i+24 <= rtcm->len*8)
if (i + 24 <= rtcm->len * 8)
{
week = getbitu(rtcm->buff, i, 10); i += 10;
hour = getbitu(rtcm->buff, i, 8); i += 8;
week = getbitu(rtcm->buff, i, 10);
i += 10;
hour = getbitu(rtcm->buff, i, 8);
i += 8;
leaps = getbitu(rtcm->buff, i, 6);
}
else
@@ -178,7 +189,7 @@ int decode_type14(rtcm_t *rtcm)
return -1;
}
week = adjgpsweek(week);
rtcm->time = gpst2time(week, hour*3600.0+zcnt*0.6);
rtcm->time = gpst2time(week, hour * 3600.0 + zcnt * 0.6);
rtcm->nav.leaps = leaps;
return 6;
}
@@ -191,9 +202,10 @@ int decode_type16(rtcm_t *rtcm)
trace(4, "decode_type16: len=%d\n", rtcm->len);
while (i+8 <= rtcm->len*8 && n<90)
while (i + 8 <= rtcm->len * 8 && n < 90)
{
rtcm->msg[n++] = getbitu(rtcm->buff, i, 8); i += 8;
rtcm->msg[n++] = getbitu(rtcm->buff, i, 8);
i += 8;
}
rtcm->msg[n] = '\0';
@@ -205,45 +217,73 @@ int decode_type16(rtcm_t *rtcm)
/* decode type 17: gps ephemerides -------------------------------------------*/
int decode_type17(rtcm_t *rtcm)
{
eph_t eph = {0, -1, -1, 0, 0, 0, 0, 0, {0,0.0}, {0,0.0}, {0,0.0},
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, 0.0, 0.0};
eph_t eph = {0, -1, -1, 0, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0, 0.0},
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, 0.0, 0.0};
double toc, sqrtA;
int i = 48, week, prn, sat;
trace(4, "decode_type17: len=%d\n", rtcm->len);
if (i+480 <= rtcm->len*8)
if (i + 480 <= rtcm->len * 8)
{
week = getbitu(rtcm->buff, i, 10); i += 10;
eph.idot = getbits(rtcm->buff, i, 14)*TWO_N43*SC2RAD; i += 14;
eph.iode = getbitu(rtcm->buff, i, 8); i += 8;
toc = getbitu(rtcm->buff, i, 16)*16.0; i += 16;
eph.f1 = getbits(rtcm->buff, i, 16)*TWO_N43; i += 16;
eph.f2 = getbits(rtcm->buff, i, 8)*TWO_N55; i += 8;
eph.crs = getbits(rtcm->buff, i, 16)*TWO_N5; i += 16;
eph.deln = getbits(rtcm->buff, i, 16)*TWO_N43*SC2RAD; i += 16;
eph.cuc = getbits(rtcm->buff, i, 16)*TWO_N29; i += 16;
eph.e = getbitu(rtcm->buff, i, 32)*TWO_N33; i += 32;
eph.cus = getbits(rtcm->buff, i, 16); i += 16;
sqrtA = getbitu(rtcm->buff, i, 32)*TWO_N19; i += 32;
eph.toes = getbitu(rtcm->buff, i, 16); i += 16;
eph.OMG0 = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32;
eph.cic = getbits(rtcm->buff, i, 16)*TWO_N29; i += 16;
eph.i0 = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32;
eph.cis = getbits(rtcm->buff, i, 16)*TWO_N29; i += 16;
eph.omg = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32;
eph.crc = getbits(rtcm->buff, i, 16)*TWO_N5; i += 16;
eph.OMGd = getbits(rtcm->buff, i, 24)*TWO_N43*SC2RAD; i += 24;
eph.M0 = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32;
eph.iodc = getbitu(rtcm->buff, i, 10); i += 10;
eph.f0 = getbits(rtcm->buff, i, 22)*TWO_N31; i += 22;
prn = getbitu(rtcm->buff, i, 5); i += 5+3;
eph.tgd[0] = getbits(rtcm->buff, i, 8)*TWO_N31; i += 8;
eph.code = getbitu(rtcm->buff, i, 2); i += 2;
eph.sva = getbitu(rtcm->buff, i, 4); i += 4;
eph.svh = getbitu(rtcm->buff, i, 6); i += 6;
eph.flag = getbitu(rtcm->buff, i, 1);
week = getbitu(rtcm->buff, i, 10);
i += 10;
eph.idot = getbits(rtcm->buff, i, 14) * TWO_N43 * SC2RAD;
i += 14;
eph.iode = getbitu(rtcm->buff, i, 8);
i += 8;
toc = getbitu(rtcm->buff, i, 16) * 16.0;
i += 16;
eph.f1 = getbits(rtcm->buff, i, 16) * TWO_N43;
i += 16;
eph.f2 = getbits(rtcm->buff, i, 8) * TWO_N55;
i += 8;
eph.crs = getbits(rtcm->buff, i, 16) * TWO_N5;
i += 16;
eph.deln = getbits(rtcm->buff, i, 16) * TWO_N43 * SC2RAD;
i += 16;
eph.cuc = getbits(rtcm->buff, i, 16) * TWO_N29;
i += 16;
eph.e = getbitu(rtcm->buff, i, 32) * TWO_N33;
i += 32;
eph.cus = getbits(rtcm->buff, i, 16);
i += 16;
sqrtA = getbitu(rtcm->buff, i, 32) * TWO_N19;
i += 32;
eph.toes = getbitu(rtcm->buff, i, 16);
i += 16;
eph.OMG0 = getbits(rtcm->buff, i, 32) * TWO_N31 * SC2RAD;
i += 32;
eph.cic = getbits(rtcm->buff, i, 16) * TWO_N29;
i += 16;
eph.i0 = getbits(rtcm->buff, i, 32) * TWO_N31 * SC2RAD;
i += 32;
eph.cis = getbits(rtcm->buff, i, 16) * TWO_N29;
i += 16;
eph.omg = getbits(rtcm->buff, i, 32) * TWO_N31 * SC2RAD;
i += 32;
eph.crc = getbits(rtcm->buff, i, 16) * TWO_N5;
i += 16;
eph.OMGd = getbits(rtcm->buff, i, 24) * TWO_N43 * SC2RAD;
i += 24;
eph.M0 = getbits(rtcm->buff, i, 32) * TWO_N31 * SC2RAD;
i += 32;
eph.iodc = getbitu(rtcm->buff, i, 10);
i += 10;
eph.f0 = getbits(rtcm->buff, i, 22) * TWO_N31;
i += 22;
prn = getbitu(rtcm->buff, i, 5);
i += 5 + 3;
eph.tgd[0] = getbits(rtcm->buff, i, 8) * TWO_N31;
i += 8;
eph.code = getbitu(rtcm->buff, i, 2);
i += 2;
eph.sva = getbitu(rtcm->buff, i, 4);
i += 4;
eph.svh = getbitu(rtcm->buff, i, 6);
i += 6;
eph.flag = getbitu(rtcm->buff, i, 1);
}
else
{
@@ -257,8 +297,8 @@ int decode_type17(rtcm_t *rtcm)
eph.toe = gpst2time(eph.week, eph.toes);
eph.toc = gpst2time(eph.week, toc);
eph.ttr = rtcm->time;
eph.A = sqrtA*sqrtA;
rtcm->nav.eph[sat-1] = eph;
eph.A = sqrtA * sqrtA;
rtcm->nav.eph[sat - 1] = eph;
rtcm->ephsat = sat;
return 2;
}
@@ -273,56 +313,64 @@ int decode_type18(rtcm_t *rtcm)
trace(4, "decode_type18: len=%d\n", rtcm->len);
if (i+24 <= rtcm->len*8)
if (i + 24 <= rtcm->len * 8)
{
freq = getbitu(rtcm->buff, i, 2); i += 2+2;
usec = getbitu(rtcm->buff, i, 20); i += 20;
freq = getbitu(rtcm->buff, i, 2);
i += 2 + 2;
usec = getbitu(rtcm->buff, i, 20);
i += 20;
}
else
{
trace(2, "rtcm2 18 length error: len=%d\n", rtcm->len);
return -1;
}
if (freq&0x1)
if (freq & 0x1)
{
trace(2, "rtcm2 18 not supported frequency: freq=%d\n", freq);
return -1;
}
freq>>= 1;
freq >>= 1;
while (i+48 <= rtcm->len*8 && rtcm->obs.n<MAXOBS)
while (i + 48 <= rtcm->len * 8 && rtcm->obs.n < MAXOBS)
{
sync = getbitu(rtcm->buff, i, 1); i += 1;
code = getbitu(rtcm->buff, i, 1); i += 1;
sys = getbitu(rtcm->buff, i, 1); i += 1;
prn = getbitu(rtcm->buff, i, 5); i += 5+3;
loss = getbitu(rtcm->buff, i, 5); i += 5;
cp = getbits(rtcm->buff, i, 32); i += 32;
sync = getbitu(rtcm->buff, i, 1);
i += 1;
code = getbitu(rtcm->buff, i, 1);
i += 1;
sys = getbitu(rtcm->buff, i, 1);
i += 1;
prn = getbitu(rtcm->buff, i, 5);
i += 5 + 3;
loss = getbitu(rtcm->buff, i, 5);
i += 5;
cp = getbits(rtcm->buff, i, 32);
i += 32;
if (prn == 0) prn = 32;
if (!(sat = satno(sys?SYS_GLO:SYS_GPS, prn)))
if (!(sat = satno(sys ? SYS_GLO : SYS_GPS, prn)))
{
trace(2, "rtcm2 18 satellite number error: sys=%d prn=%d\n", sys, prn);
continue;
}
time = timeadd(rtcm->time, usec*1E-6);
time = timeadd(rtcm->time, usec * 1E-6);
if (sys) time = utc2gpst(time); /* convert glonass time to gpst */
tt = timediff(rtcm->obs.data[0].time, time);
if (rtcm->obsflag||fabs(tt)>1E-9)
if (rtcm->obsflag || fabs(tt) > 1E-9)
{
rtcm->obs.n = rtcm->obsflag = 0;
}
if ((index = obsindex(&rtcm->obs, time, sat)) >= 0)
{
rtcm->obs.data[index].L[freq] = -cp/256.0;
rtcm->obs.data[index].LLI[freq] = rtcm->loss[sat-1][freq] != loss;
rtcm->obs.data[index].L[freq] = -cp / 256.0;
rtcm->obs.data[index].LLI[freq] = rtcm->loss[sat - 1][freq] != loss;
rtcm->obs.data[index].code[freq] =
!freq?(code?CODE_L1P:CODE_L1C):(code?CODE_L2P:CODE_L2C);
rtcm->loss[sat-1][freq] = loss;
!freq ? (code ? CODE_L1P : CODE_L1C) : (code ? CODE_L2P : CODE_L2C);
rtcm->loss[sat - 1][freq] = loss;
}
}
rtcm->obsflag = !sync;
return sync?0:1;
return sync ? 0 : 1;
}
@@ -335,53 +383,60 @@ int decode_type19(rtcm_t *rtcm)
trace(4, "decode_type19: len=%d\n", rtcm->len);
if (i+24 <= rtcm->len*8)
if (i + 24 <= rtcm->len * 8)
{
freq = getbitu(rtcm->buff, i, 2); i += 2+2;
usec = getbitu(rtcm->buff, i, 20); i += 20;
freq = getbitu(rtcm->buff, i, 2);
i += 2 + 2;
usec = getbitu(rtcm->buff, i, 20);
i += 20;
}
else
{
trace(2, "rtcm2 19 length error: len=%d\n", rtcm->len);
return -1;
}
if (freq&0x1)
if (freq & 0x1)
{
trace(2, "rtcm2 19 not supported frequency: freq=%d\n", freq);
return -1;
}
freq>>=1;
freq >>= 1;
while (i+48 <= rtcm->len*8 && rtcm->obs.n<MAXOBS)
while (i + 48 <= rtcm->len * 8 && rtcm->obs.n < MAXOBS)
{
sync = getbitu(rtcm->buff, i, 1); i += 1;
code = getbitu(rtcm->buff, i, 1); i += 1;
sys = getbitu(rtcm->buff, i, 1); i += 1;
prn = getbitu(rtcm->buff, i, 5); i += 5+8;
pr = getbitu(rtcm->buff, i, 32); i += 32;
sync = getbitu(rtcm->buff, i, 1);
i += 1;
code = getbitu(rtcm->buff, i, 1);
i += 1;
sys = getbitu(rtcm->buff, i, 1);
i += 1;
prn = getbitu(rtcm->buff, i, 5);
i += 5 + 8;
pr = getbitu(rtcm->buff, i, 32);
i += 32;
if (prn == 0) prn = 32;
if (!(sat = satno(sys?SYS_GLO:SYS_GPS, prn)))
if (!(sat = satno(sys ? SYS_GLO : SYS_GPS, prn)))
{
trace(2, "rtcm2 19 satellite number error: sys=%d prn=%d\n", sys, prn);
continue;
}
time = timeadd(rtcm->time, usec*1E-6);
time = timeadd(rtcm->time, usec * 1E-6);
if (sys) time = utc2gpst(time); /* convert glonass time to gpst */
tt = timediff(rtcm->obs.data[0].time, time);
if (rtcm->obsflag||fabs(tt)>1E-9)
if (rtcm->obsflag || fabs(tt) > 1E-9)
{
rtcm->obs.n = rtcm->obsflag = 0;
}
if ((index = obsindex(&rtcm->obs, time, sat)) >= 0)
{
rtcm->obs.data[index].P[freq] = pr*0.02;
rtcm->obs.data[index].P[freq] = pr * 0.02;
rtcm->obs.data[index].code[freq] =
!freq?(code?CODE_L1P:CODE_L1C):(code?CODE_L2P:CODE_L2C);
!freq ? (code ? CODE_L1P : CODE_L1C) : (code ? CODE_L2P : CODE_L2C);
}
}
rtcm->obsflag = !sync;
return sync?0:1;
return sync ? 0 : 1;
}
@@ -393,31 +448,38 @@ int decode_type22(rtcm_t *rtcm)
trace(4, "decode_type22: len=%d\n", rtcm->len);
if (i+24 <= rtcm->len*8)
if (i + 24 <= rtcm->len * 8)
{
del[0][0] = getbits(rtcm->buff, i, 8)/25600.0; i += 8;
del[0][1] = getbits(rtcm->buff, i, 8)/25600.0; i += 8;
del[0][2] = getbits(rtcm->buff, i, 8)/25600.0; i += 8;
del[0][0] = getbits(rtcm->buff, i, 8) / 25600.0;
i += 8;
del[0][1] = getbits(rtcm->buff, i, 8) / 25600.0;
i += 8;
del[0][2] = getbits(rtcm->buff, i, 8) / 25600.0;
i += 8;
}
else
{
trace(2, "rtcm2 22 length error: len=%d\n", rtcm->len);
return -1;
}
if (i+24 <= rtcm->len*8)
if (i + 24 <= rtcm->len * 8)
{
i += 5; noh = getbits(rtcm->buff, i, 1); i += 1;
hgt = noh?0.0:getbitu(rtcm->buff, i, 18)/25600.0;
i += 5;
noh = getbits(rtcm->buff, i, 1);
i += 1;
hgt = noh ? 0.0 : getbitu(rtcm->buff, i, 18) / 25600.0;
i += 18;
}
if (i+24 <= rtcm->len*8)
if (i + 24 <= rtcm->len * 8)
{
del[1][0] = getbits(rtcm->buff, i, 8)/1600.0; i += 8;
del[1][1] = getbits(rtcm->buff, i, 8)/1600.0; i += 8;
del[1][2] = getbits(rtcm->buff, i, 8)/1600.0;
del[1][0] = getbits(rtcm->buff, i, 8) / 1600.0;
i += 8;
del[1][1] = getbits(rtcm->buff, i, 8) / 1600.0;
i += 8;
del[1][2] = getbits(rtcm->buff, i, 8) / 1600.0;
}
rtcm->sta.deltype = 1; /* xyz */
for (j = 0; j<3; j++) rtcm->sta.del[j] = del[0][j];
for (j = 0; j < 3; j++) rtcm->sta.del[j] = del[0][j];
rtcm->sta.hgt = hgt;
return 5;
}
@@ -487,7 +549,7 @@ int decode_rtcm2(rtcm_t *rtcm)
trace(3, "decode_rtcm2: type=%2d len=%3d\n", type, rtcm->len);
if ((zcnt = getbitu(rtcm->buff, 24, 13)*0.6) >= 3600.0)
if ((zcnt = getbitu(rtcm->buff, 24, 13) * 0.6) >= 3600.0)
{
trace(2, "rtcm2 modified z-count error: zcnt=%.1f\n", zcnt);
return -1;
@@ -495,20 +557,20 @@ int decode_rtcm2(rtcm_t *rtcm)
adjhour(rtcm, zcnt);
staid = getbitu(rtcm->buff, 14, 10);
seqno = getbitu(rtcm->buff, 37, 3);
stah = getbitu(rtcm->buff, 45, 3);
if (seqno-rtcm->seqno != 1 && seqno-rtcm->seqno != -7)
stah = getbitu(rtcm->buff, 45, 3);
if (seqno - rtcm->seqno != 1 && seqno - rtcm->seqno != -7)
{
trace(2, "rtcm2 message outage: seqno=%d->%d\n", rtcm->seqno, seqno);
}
rtcm->seqno = seqno;
rtcm->stah = stah;
rtcm->stah = stah;
if (rtcm->outtype)
{
sprintf(rtcm->msgtype, "RTCM %2d (%4d) zcnt=%7.1f staid=%3d seqno=%d",
type, rtcm->len, zcnt, staid, seqno);
type, rtcm->len, zcnt, staid, seqno);
}
if (type == 3||type == 22||type == 23||type == 24)
if (type == 3 || type == 22 || type == 23 || type == 24)
{
if (rtcm->staid != 0 && staid != rtcm->staid)
{
@@ -522,28 +584,65 @@ int decode_rtcm2(rtcm_t *rtcm)
return -1;
}
switch (type)
{
case 1: ret = decode_type1 (rtcm); break;
case 3: ret = decode_type3 (rtcm); break;
case 9: ret = decode_type1 (rtcm); break;
case 14: ret = decode_type14(rtcm); break;
case 16: ret = decode_type16(rtcm); break;
case 17: ret = decode_type17(rtcm); break;
case 18: ret = decode_type18(rtcm); break;
case 19: ret = decode_type19(rtcm); break;
case 22: ret = decode_type22(rtcm); break;
case 23: ret = decode_type23(rtcm); break; /* not supported */
case 24: ret = decode_type24(rtcm); break; /* not supported */
case 31: ret = decode_type31(rtcm); break; /* not supported */
case 32: ret = decode_type32(rtcm); break; /* not supported */
case 34: ret = decode_type34(rtcm); break; /* not supported */
case 36: ret = decode_type36(rtcm); break; /* not supported */
case 37: ret = decode_type37(rtcm); break; /* not supported */
case 59: ret = decode_type59(rtcm); break; /* not supported */
}
{
case 1:
ret = decode_type1(rtcm);
break;
case 3:
ret = decode_type3(rtcm);
break;
case 9:
ret = decode_type1(rtcm);
break;
case 14:
ret = decode_type14(rtcm);
break;
case 16:
ret = decode_type16(rtcm);
break;
case 17:
ret = decode_type17(rtcm);
break;
case 18:
ret = decode_type18(rtcm);
break;
case 19:
ret = decode_type19(rtcm);
break;
case 22:
ret = decode_type22(rtcm);
break;
case 23:
ret = decode_type23(rtcm);
break; /* not supported */
case 24:
ret = decode_type24(rtcm);
break; /* not supported */
case 31:
ret = decode_type31(rtcm);
break; /* not supported */
case 32:
ret = decode_type32(rtcm);
break; /* not supported */
case 34:
ret = decode_type34(rtcm);
break; /* not supported */
case 36:
ret = decode_type36(rtcm);
break; /* not supported */
case 37:
ret = decode_type37(rtcm);
break; /* not supported */
case 59:
ret = decode_type59(rtcm);
break; /* not supported */
}
if (ret >= 0)
{
if (1 <= type && type <= 99) rtcm->nmsg2[type]++; else rtcm->nmsg2[0]++;
if (1 <= type && type <= 99)
rtcm->nmsg2[type]++;
else
rtcm->nmsg2[0]++;
}
return ret;
}

2829
src/algorithms/libs/rtklib/rtklib_rtcm3.cc
View File

File diff suppressed because it is too large Load Diff

46
src/algorithms/libs/rtklib/rtklib_rtcm3.h
View File

@@ -58,56 +58,48 @@
/* constants -----------------------------------------------------------------*/
const double PRUNIT_GPS = 299792.458; /* rtcm ver.3 unit of gps pseudorange (m) */
const double PRUNIT_GLO = 599584.916; /* rtcm ver.3 unit of glonass pseudorange (m) */
const double RANGE_MS = SPEED_OF_LIGHT * 0.001; /* range in 1 ms */
const double PRUNIT_GPS = 299792.458; /* rtcm ver.3 unit of gps pseudorange (m) */
const double PRUNIT_GLO = 599584.916; /* rtcm ver.3 unit of glonass pseudorange (m) */
const double RANGE_MS = SPEED_OF_LIGHT * 0.001; /* range in 1 ms */
/* ssr update intervals ------------------------------------------------------*/
const double ssrudint[16] = {
1, 2, 5, 10, 15, 30, 60, 120, 240, 300, 600, 900, 1800, 3600, 7200, 10800
};
1, 2, 5, 10, 15, 30, 60, 120, 240, 300, 600, 900, 1800, 3600, 7200, 10800};
/* ssr 3 and 7 signal and tracking mode ids ----------------------------------*/
const int codes_gps[] = {
CODE_L1C, CODE_L1P, CODE_L1W, CODE_L1Y, CODE_L1M, CODE_L2C, CODE_L2D, CODE_L2S,
CODE_L2L, CODE_L2X, CODE_L2P, CODE_L2W, CODE_L2Y, CODE_L2M, CODE_L5I, CODE_L5Q,
CODE_L5X
};
CODE_L5X};
const int codes_glo[] = {
CODE_L1C, CODE_L1P, CODE_L2C, CODE_L2P
};
CODE_L1C, CODE_L1P, CODE_L2C, CODE_L2P};
const int codes_gal[] = {
CODE_L1A, CODE_L1B, CODE_L1C, CODE_L1X, CODE_L1Z, CODE_L5I, CODE_L5Q, CODE_L5X,
CODE_L7I, CODE_L7Q, CODE_L7X, CODE_L8I, CODE_L8Q, CODE_L8X, CODE_L6A, CODE_L6B,
CODE_L6C, CODE_L6X, CODE_L6Z
};
CODE_L6C, CODE_L6X, CODE_L6Z};
const int codes_qzs[] = {
CODE_L1C, CODE_L1S, CODE_L1L, CODE_L2S, CODE_L2L, CODE_L2X, CODE_L5I, CODE_L5Q,
CODE_L5X, CODE_L6S, CODE_L6L, CODE_L6X, CODE_L1X
};
CODE_L5X, CODE_L6S, CODE_L6L, CODE_L6X, CODE_L1X};
const int codes_bds[] = {
CODE_L1I, CODE_L1Q, CODE_L1X, CODE_L7I, CODE_L7Q, CODE_L7X, CODE_L6I, CODE_L6Q,
CODE_L6X
};
CODE_L6X};
const int codes_sbs[] = {
CODE_L1C, CODE_L5I, CODE_L5Q, CODE_L5X
};
CODE_L1C, CODE_L5I, CODE_L5Q, CODE_L5X};
double getbitg(const unsigned char *buff, int pos, int len);
double getbitg(const unsigned char *buff, int pos, int len);
void adjweek(rtcm_t *rtcm, double tow);
@@ -206,13 +198,13 @@ int decode_type1047(rtcm_t *rtcm);
int decode_type1063(rtcm_t *rtcm);
int decode_ssr1_head(rtcm_t *rtcm, int sys, int *sync, int *iod,
double *udint, int *refd, int *hsize);
double *udint, int *refd, int *hsize);
int decode_ssr2_head(rtcm_t *rtcm, int sys, int *sync, int *iod,
double *udint, int *hsize);
double *udint, int *hsize);
int decode_ssr7_head(rtcm_t *rtcm, int sys, int *sync, int *iod,
double *udint, int *dispe, int *mw, int *hsize);
double *udint, int *dispe, int *mw, int *hsize);
int decode_ssr1(rtcm_t *rtcm, int sys);
@@ -229,15 +221,15 @@ int decode_ssr6(rtcm_t *rtcm, int sys);
int decode_ssr7(rtcm_t *rtcm, int sys);
void sigindex(int sys, const unsigned char *code, const int *freq, int n,
const char *opt, int *ind);
const char *opt, int *ind);
void save_msm_obs(rtcm_t *rtcm, int sys, msm_h_t *h, const double *r,
const double *pr, const double *cp, const double *rr,
const double *rrf, const double *cnr, const int *lock,
const int *ex, const int *half);
const double *pr, const double *cp, const double *rr,
const double *rrf, const double *cnr, const int *lock,
const int *ex, const int *half);
int decode_msm_head(rtcm_t *rtcm, int sys, int *sync, int *iod,
msm_h_t *h, int *hsize);
msm_h_t *h, int *hsize);
int decode_msm0(rtcm_t *rtcm, int sys);

2456
src/algorithms/libs/rtklib/rtklib_rtkcmn.cc
View File

File diff suppressed because it is too large Load Diff

86
src/algorithms/libs/rtklib/rtklib_rtkcmn.h
View File

@@ -82,22 +82,38 @@
#include <glog/logging.h>
/* coordinate rotation matrix ------------------------------------------------*/
#define Rx(t,X) do { \
(X)[0]=1.0; (X)[1]=(X)[2]=(X)[3]=(X)[6]=0.0; \
(X)[4]=(X)[8]=cos(t); (X)[7]=sin(t); (X)[5]=-(X)[7]; \
} while (0)
#define Ry(t,X) do { \
(X)[4]=1.0; (X)[1]=(X)[3]=(X)[5]=(X)[7]=0.0; \
(X)[0]=(X)[8]=cos(t); (X)[2]=sin(t); (X)[6]=-(X)[2]; \
} while (0)
#define Rz(t,X) do { \
(X)[8]=1.0; (X)[2]=(X)[5]=(X)[6]=(X)[7]=0.0; \
(X)[0]=(X)[4]=cos(t); (X)[3]=sin(t); (X)[1]=-(X)[3]; \
} while (0)
#define Rx(t, X) \
do \
{ \
(X)[0] = 1.0; \
(X)[1] = (X)[2] = (X)[3] = (X)[6] = 0.0; \
(X)[4] = (X)[8] = cos(t); \
(X)[7] = sin(t); \
(X)[5] = -(X)[7]; \
} \
while (0)
#define Ry(t, X) \
do \
{ \
(X)[4] = 1.0; \
(X)[1] = (X)[3] = (X)[5] = (X)[7] = 0.0; \
(X)[0] = (X)[8] = cos(t); \
(X)[2] = sin(t); \
(X)[6] = -(X)[2]; \
} \
while (0)
#define Rz(t, X) \
do \
{ \
(X)[8] = 1.0; \
(X)[2] = (X)[5] = (X)[6] = (X)[7] = 0.0; \
(X)[0] = (X)[4] = cos(t); \
(X)[3] = sin(t); \
(X)[1] = -(X)[3]; \
} \
while (0)
void fatalerr(const char *format, ...);
@@ -106,7 +122,7 @@ int satsys(int sat, int *prn);
int satid2no(const char *id);
void satno2id(int sat, char *id);
int satexclude(int sat, int svh, const prcopt_t *opt);
int testsnr(int base, int freq, double el, double snr,const snrmask_t *mask);
int testsnr(int base, int freq, double el, double snr, const snrmask_t *mask);
unsigned char obs2code(const char *obs, int *freq);
char *code2obs(unsigned char code, int *freq);
void setcodepri(int sys, int freq, const char *pri);
@@ -129,21 +145,21 @@ void cross3(const double *a, const double *b, double *c);
int normv3(const double *a, double *b);
void matcpy(double *A, const double *B, int n, int m);
void matmul(const char *tr, int n, int k, int m, double alpha,
const double *A, const double *B, double beta, double *C);
const double *A, const double *B, double beta, double *C);
int matinv(double *A, int n);
int solve(const char *tr, const double *A, const double *Y, int n,
int m, double *X);
int m, double *X);
int lsq(const double *A, const double *y, int n, int m, double *x,
double *Q);
double *Q);
int filter_(const double *x, const double *P, const double *H,
const double *v, const double *R, int n, int m,
double *xp, double *Pp);
const double *v, const double *R, int n, int m,
double *xp, double *Pp);
int filter(double *x, double *P, const double *H, const double *v,
const double *R, int n, int m);
const double *R, int n, int m);
int smoother(const double *xf, const double *Qf, const double *xb,
const double *Qb, int n, double *xs, double *Qs);
const double *Qb, int n, double *xs, double *Qs);
void matfprint(const double A[], int n, int m, int p, int q, FILE *fp);
void matsprint(const double A[], int n, int m, int p, int q, std::string & buffer);
void matsprint(const double A[], int n, int m, int p, int q, std::string &buffer);
void matprint(const double A[], int n, int m, int p, int q);
double str2num(const char *s, int i, int n);
int str2time(const char *s, int i, int n, gtime_t *t);
@@ -193,7 +209,7 @@ int readngspcv(const char *file, pcvs_t *pcvs);
int readantex(const char *file, pcvs_t *pcvs);
int readpcv(const char *file, pcvs_t *pcvs);
pcv_t *searchpcv(int sat, const char *type, gtime_t time,
const pcvs_t *pcvs);
const pcvs_t *pcvs);
void readpos(const char *file, const char *rcv, double *pos);
int readblqrecord(FILE *fp, double *odisp);
int readblq(const char *file, const char *sta, double *odisp);
@@ -218,8 +234,8 @@ void traceopen(const char *file);
void traceclose(void);
void tracelevel(int level);
void traceswap(void);
void trace (int level, const char *format, ...);
void tracet (int level, const char *format, ...);
void trace(int level, const char *format, ...);
void tracet(int level, const char *format, ...);
void tracemat(int level, const double *A, int n, int m, int p, int q);
void traceobs(int level, const obsd_t *obs, int n);
//void tracenav(int level, const nav_t *nav);
@@ -233,9 +249,9 @@ int execcmd(const char *cmd);
void createdir(const char *path);
int repstr(char *str, const char *pat, const char *rep);
int reppath(const char *path, char *rpath, gtime_t time, const char *rov,
const char *base);
const char *base);
int reppaths(const char *path, char *rpath[], int nmax, gtime_t ts,
gtime_t te, const char *rov, const char *base);
gtime_t te, const char *rov, const char *base);
double satwavelen(int sat, int frq, const nav_t *nav);
double geodist(const double *rs, const double *rr, double *e);
double satazel(const double *pos, const double *e, double *azel);
@@ -243,27 +259,27 @@ double satazel(const double *pos, const double *e, double *azel);
//#define SQRT(x) ((x)<0.0?0.0:sqrt(x))
void dops(int ns, const double *azel, double elmin, double *dop);
double ionmodel(gtime_t t, const double *ion, const double *pos,
const double *azel);
const double *azel);
double ionmapf(const double *pos, const double *azel);
double ionppp(const double *pos, const double *azel, double re,
double hion, double *posp);
double hion, double *posp);
double tropmodel(gtime_t time, const double *pos, const double *azel,
double humi);
double humi);
double interpc(const double coef[], double lat);
double mapf(double el, double a, double b, double c);
double nmf(gtime_t time, const double pos[], const double azel[],
double *mapfw);
double *mapfw);
double tropmapf(gtime_t time, const double pos[], const double azel[],
double *mapfw);
double *mapfw);
double interpvar(double ang, const double *var);
void antmodel(const pcv_t *pcv, const double *del, const double *azel,
int opt, double *dant);
int opt, double *dant);
void antmodel_s(const pcv_t *pcv, double nadir, double *dant);
void sunmoonpos_eci(gtime_t tut, double *rsun, double *rmoon);
void sunmoonpos(gtime_t tutc, const double *erpv, double *rsun,
double *rmoon, double *gmst);
double *rmoon, double *gmst);
void csmooth(obs_t *obs, int ns);
int rtk_uncompress(const char *file, char *uncfile);
int expath(const char *path, char *paths[], int nmax);

1266
src/algorithms/libs/rtklib/rtklib_rtkpos.cc
View File

File diff suppressed because it is too large Load Diff

73
src/algorithms/libs/rtklib/rtklib_rtkpos.h
View File

@@ -58,29 +58,29 @@
#include "rtklib_rtkcmn.h"
/* constants/macros ----------------------------------------------------------*/
const double VAR_POS = std::pow(30.0, 2.0); /* initial variance of receiver pos (m^2) */
const double VAR_VEL = std::pow(10.0, 2.0); /* initial variance of receiver vel ((m/s)^2) */
const double VAR_ACC = std::pow(10.0, 2.0); /* initial variance of receiver acc ((m/ss)^2) */
const double VAR_HWBIAS = std::pow(1.0, 2.0); /* initial variance of h/w bias ((m/MHz)^2) */
const double VAR_GRA = std::pow(0.001, 2.0); /* initial variance of gradient (m^2) */
const double INIT_ZWD = 0.15; /* initial zwd (m) */
const double VAR_POS = std::pow(30.0, 2.0); /* initial variance of receiver pos (m^2) */
const double VAR_VEL = std::pow(10.0, 2.0); /* initial variance of receiver vel ((m/s)^2) */
const double VAR_ACC = std::pow(10.0, 2.0); /* initial variance of receiver acc ((m/ss)^2) */
const double VAR_HWBIAS = std::pow(1.0, 2.0); /* initial variance of h/w bias ((m/MHz)^2) */
const double VAR_GRA = std::pow(0.001, 2.0); /* initial variance of gradient (m^2) */
const double INIT_ZWD = 0.15; /* initial zwd (m) */
const double PRN_HWBIA = 1E-6; /* process noise of h/w bias (m/MHz/sqrt(s)) */
const double PRN_HWBIA = 1E-6; /* process noise of h/w bias (m/MHz/sqrt(s)) */
const double MAXAC = 30.0; /* max accel for doppler slip detection (m/s^2) */
const double VAR_HOLDAMB = 0.001; /* constraint to hold ambiguity (cycle^2) */
const double VAR_HOLDAMB = 0.001; /* constraint to hold ambiguity (cycle^2) */
const double TTOL_MOVEB = (1.0+2*DTTOL);
const double TTOL_MOVEB = (1.0 + 2 * DTTOL);
/* time sync tolerance for moving-baseline (s) */
/* number of parameters (pos,ionos,tropos,hw-bias,phase-bias,real,estimated) */
/* state variable index */
#define II_RTK(s,opt) (NP_RTK(opt)+(s)-1) /* ionos (s:satellite no) */
#define IT_RTK(r,opt) (NP_RTK(opt)+NI_RTK(opt)+NT_RTK(opt)/2*(r)) /* tropos (r:0=rov,1:ref) */
#define IL_RTK(f,opt) (NP_RTK(opt)+NI_RTK(opt)+NT_RTK(opt)+(f)) /* receiver h/w bias */
#define IB_RTK(s,f,opt) (NR_RTK(opt)+MAXSAT*(f)+(s)-1) /* phase bias (s:satno,f:freq) */
#define II_RTK(s, opt) (NP_RTK(opt) + (s)-1) /* ionos (s:satellite no) */
#define IT_RTK(r, opt) (NP_RTK(opt) + NI_RTK(opt) + NT_RTK(opt) / 2 * (r)) /* tropos (r:0=rov,1:ref) */
#define IL_RTK(f, opt) (NP_RTK(opt) + NI_RTK(opt) + NT_RTK(opt) + (f)) /* receiver h/w bias */
#define IB_RTK(s, f, opt) (NR_RTK(opt) + MAXSAT * (f) + (s)-1) /* phase bias (s:satno,f:freq) */
int rtkopenstat(const char *file, int level);
@@ -101,7 +101,7 @@ double gfobs_L1L2(const obsd_t *obs, int i, int j, const double *lam);
double gfobs_L1L5(const obsd_t *obs, int i, int j, const double *lam);
double varerr(int sat, int sys, double el, double bl, double dt, int f,
const prcopt_t *opt);
const prcopt_t *opt);
double baseline(const double *ru, const double *rb, double *dr);
@@ -109,7 +109,7 @@ double baseline(const double *ru, const double *rb, double *dr);
void initx_rtk(rtk_t *rtk, double xi, double var, int i);
int selsat(const obsd_t *obs, double *azel, int nu, int nr,
const prcopt_t *opt, int *sat, int *iu, int *ir);
const prcopt_t *opt, int *sat, int *iu, int *ir);
void udpos(rtk_t *rtk, double tt);
@@ -121,53 +121,53 @@ void udrcvbias(rtk_t *rtk, double tt);
void detslp_ll(rtk_t *rtk, const obsd_t *obs, int i, int rcv);
void detslp_gf_L1L2(rtk_t *rtk, const obsd_t *obs, int i, int j,
const nav_t *nav);
const nav_t *nav);
void detslp_gf_L1L5(rtk_t *rtk, const obsd_t *obs, int i, int j,
const nav_t *nav);
const nav_t *nav);
void detslp_dop(rtk_t *rtk, const obsd_t *obs, int i, int rcv,
const nav_t *nav);
const nav_t *nav);
void udbias(rtk_t *rtk, double tt, const obsd_t *obs, const int *sat,
const int *iu, const int *ir, int ns, const nav_t *nav);
const int *iu, const int *ir, int ns, const nav_t *nav);
void udstate(rtk_t *rtk, const obsd_t *obs, const int *sat,
const int *iu, const int *ir, int ns, const nav_t *nav);
const int *iu, const int *ir, int ns, const nav_t *nav);
void zdres_sat(int base, double r, const obsd_t *obs, const nav_t *nav,
const double *azel, const double *dant,
const prcopt_t *opt, double *y);
const double *azel, const double *dant,
const prcopt_t *opt, double *y);
int zdres(int base, const obsd_t *obs, int n, const double *rs,
const double *dts, const int *svh, const nav_t *nav,
const double *rr, const prcopt_t *opt, int index, double *y,
double *e, double *azel);
const double *dts, const int *svh, const nav_t *nav,
const double *rr, const prcopt_t *opt, int index, double *y,
double *e, double *azel);
int validobs(int i, int j, int f, int nf, double *y);
void ddcov(const int *nb, int n, const double *Ri, const double *Rj,
int nv, double *R);
int nv, double *R);
int constbl(rtk_t *rtk, const double *x, const double *P, double *v,
double *H, double *Ri, double *Rj, int index);
double *H, double *Ri, double *Rj, int index);
double prectrop(gtime_t time, const double *pos, int r,
const double *azel, const prcopt_t *opt, const double *x,
double *dtdx);
const double *azel, const prcopt_t *opt, const double *x,
double *dtdx);
double gloicbcorr(int sat1, int sat2, const prcopt_t *opt, double lam1,
double lam2, int f);
double lam2, int f);
int test_sys(int sys, int m);
int ddres(rtk_t *rtk, const nav_t *nav, double dt, const double *x,
const double *P, const int *sat, double *y, double *e,
double *azel, const int *iu, const int *ir, int ns, double *v,
double *H, double *R, int *vflg);
const double *P, const int *sat, double *y, double *e,
double *azel, const int *iu, const int *ir, int ns, double *v,
double *H, double *R, int *vflg);
double intpres(gtime_t time, const obsd_t *obs, int n, const nav_t *nav,
rtk_t *rtk, double *y);
rtk_t *rtk, double *y);
int ddmat(rtk_t *rtk, double *D);
@@ -179,10 +179,10 @@ void holdamb(rtk_t *rtk, const double *xa);
int resamb_LAMBDA(rtk_t *rtk, double *bias, double *xa);
int valpos(rtk_t *rtk, const double *v, const double *R, const int *vflg,
int nv, double thres);
int nv, double thres);
int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr,
const nav_t *nav);
const nav_t *nav);
void rtkinit(rtk_t *rtk, const prcopt_t *opt);
@@ -191,5 +191,4 @@ void rtkfree(rtk_t *rtk);
int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav);
#endif

352
src/algorithms/libs/rtklib/rtklib_rtksvr.cc
View File

@@ -23,8 +23,8 @@ void saveoutbuf(rtksvr_t *svr, unsigned char *buff, int n, int index)
{
rtksvrlock(svr);
n = n < svr->buffsize-svr->nsb[index] ? n : svr->buffsize-svr->nsb[index];
memcpy(svr->sbuf[index]+svr->nsb[index], buff, n);
n = n < svr->buffsize - svr->nsb[index] ? n : svr->buffsize - svr->nsb[index];
memcpy(svr->sbuf[index] + svr->nsb[index], buff, n);
svr->nsb[index] += n;
rtksvrunlock(svr);
@@ -43,15 +43,15 @@ void writesol(rtksvr_t *svr, int index)
for (i = 0; i < 2; i++)
{
/* output solution */
n = outsols(buff, &svr->rtk.sol, svr->rtk.rb, svr->solopt+i);
strwrite(svr->stream+i+3, buff, n);
n = outsols(buff, &svr->rtk.sol, svr->rtk.rb, svr->solopt + i);
strwrite(svr->stream + i + 3, buff, n);
/* save output buffer */
saveoutbuf(svr, buff, n, i);
/* output extended solution */
n = outsolexs(buff, &svr->rtk.sol, svr->rtk.ssat, svr->solopt+i);
strwrite(svr->stream+i+3, buff, n);
n = outsolexs(buff, &svr->rtk.sol, svr->rtk.ssat, svr->solopt + i);
strwrite(svr->stream + i + 3, buff, n);
/* save output buffer */
saveoutbuf(svr, buff, n, i);
@@ -76,10 +76,11 @@ void writesol(rtksvr_t *svr, int index)
void updatenav(nav_t *nav)
{
int i, j;
for (i = 0; i < MAXSAT; i++) for (j = 0; j < NFREQ; j++)
{
nav->lam[i][j] = satwavelen(i+1, j, nav);
}
for (i = 0; i < MAXSAT; i++)
for (j = 0; j < NFREQ; j++)
{
nav->lam[i][j] = satwavelen(i + 1, j, nav);
}
}
@@ -90,14 +91,14 @@ void updatefcn(rtksvr_t *svr)
for (i = 0; i < MAXPRNGLO; i++)
{
sat = satno(SYS_GLO, i+1);
sat = satno(SYS_GLO, i + 1);
for (j = 0, frq = -999; j < 3; j++)
{
if (svr->raw[j].nav.geph[i].sat != sat) continue;
frq = svr->raw[j].nav.geph[i].frq;
}
if (frq < -7 || frq>6) continue;
if (frq < -7 || frq > 6) continue;
for (j = 0; j < 3; j++)
{
@@ -111,7 +112,7 @@ void updatefcn(rtksvr_t *svr)
/* update rtk server struct --------------------------------------------------*/
void updatesvr(rtksvr_t *svr, int ret, obs_t *obs, nav_t *nav, int sat,
sbsmsg_t *sbsmsg, int index, int iobs)
sbsmsg_t *sbsmsg, int index, int iobs)
{
eph_t *eph1, *eph2, *eph3;
geph_t *geph1, *geph2, *geph3;
@@ -127,10 +128,10 @@ void updatesvr(rtksvr_t *svr, int ret, obs_t *obs, nav_t *nav, int sat,
{
for (i = 0; i < obs->n; i++)
{
if (svr->rtk.opt.exsats[obs->data[i].sat-1] == 1 ||
!(satsys(obs->data[i].sat, NULL)&svr->rtk.opt.navsys)) continue;
if (svr->rtk.opt.exsats[obs->data[i].sat - 1] == 1 ||
!(satsys(obs->data[i].sat, NULL) & svr->rtk.opt.navsys)) continue;
svr->obs[index][iobs].data[n] = obs->data[i];
svr->obs[index][iobs].data[n++].rcv = index+1;
svr->obs[index][iobs].data[n++].rcv = index + 1;
}
svr->obs[index][iobs].n = n;
sortobs(&svr->obs[index][iobs]);
@@ -141,15 +142,15 @@ void updatesvr(rtksvr_t *svr, int ret, obs_t *obs, nav_t *nav, int sat,
{ /* ephemeris */
if (satsys(sat, &prn) != SYS_GLO)
{
if (!svr->navsel || svr->navsel == index+1)
if (!svr->navsel || svr->navsel == index + 1)
{
eph1 = nav->eph+sat-1;
eph2 = svr->nav.eph+sat-1;
eph3 = svr->nav.eph+sat-1+MAXSAT;
eph1 = nav->eph + sat - 1;
eph2 = svr->nav.eph + sat - 1;
eph3 = svr->nav.eph + sat - 1 + MAXSAT;
if (eph2->ttr.time == 0 ||
(eph1->iode != eph3->iode && eph1->iode != eph2->iode) ||
(timediff(eph1->toe, eph3->toe) != 0.0 &&
timediff(eph1->toe, eph2->toe) != 0.0))
(eph1->iode != eph3->iode && eph1->iode != eph2->iode) ||
(timediff(eph1->toe, eph3->toe) != 0.0 &&
timediff(eph1->toe, eph2->toe) != 0.0))
{
*eph3 = *eph2;
*eph2 = *eph1;
@@ -160,13 +161,13 @@ void updatesvr(rtksvr_t *svr, int ret, obs_t *obs, nav_t *nav, int sat,
}
else
{
if (!svr->navsel || svr->navsel == index+1)
if (!svr->navsel || svr->navsel == index + 1)
{
geph1 = nav->geph+prn-1;
geph2 = svr->nav.geph+prn-1;
geph3 = svr->nav.geph+prn-1+MAXPRNGLO;
geph1 = nav->geph + prn - 1;
geph2 = svr->nav.geph + prn - 1;
geph3 = svr->nav.geph + prn - 1 + MAXPRNGLO;
if (geph2->tof.time == 0 ||
(geph1->iode != geph3->iode && geph1->iode != geph2->iode))
(geph1->iode != geph3->iode && geph1->iode != geph2->iode))
{
*geph3 = *geph2;
*geph2 = *geph1;
@@ -187,7 +188,7 @@ void updatesvr(rtksvr_t *svr, int ret, obs_t *obs, nav_t *nav, int sat,
}
else
{
for (i = 0; i < MAXSBSMSG-1; i++) svr->sbsmsg[i] = svr->sbsmsg[i+1];
for (i = 0; i < MAXSBSMSG - 1; i++) svr->sbsmsg[i] = svr->sbsmsg[i + 1];
svr->sbsmsg[i] = *sbsmsg;
}
sbsupdatecorr(sbsmsg, &svr->nav);
@@ -224,7 +225,7 @@ void updatesvr(rtksvr_t *svr, int ret, obs_t *obs, nav_t *nav, int sat,
enu2ecef(pos, del, dr);
for (i = 0; i < 3; i++)
{
svr->rtk.rb[i] += svr->rtcm[1].sta.del[i]+dr[i];
svr->rtk.rb[i] += svr->rtcm[1].sta.del[i] + dr[i];
}
}
else
@@ -250,21 +251,21 @@ void updatesvr(rtksvr_t *svr, int ret, obs_t *obs, nav_t *nav, int sat,
svr->rtcm[index].ssr[i].update = 0;
iode = svr->rtcm[index].ssr[i].iode;
sys = satsys(i+1, &prn);
sys = satsys(i + 1, &prn);
/* check corresponding ephemeris exists */
if (sys == SYS_GPS || sys == SYS_GAL || sys == SYS_QZS)
{
if (svr->nav.eph[i].iode != iode &&
svr->nav.eph[i+MAXSAT].iode != iode)
svr->nav.eph[i + MAXSAT].iode != iode)
{
continue;
}
}
else if (sys == SYS_GLO)
{
if (svr->nav.geph[prn-1].iode != iode &&
svr->nav.geph[prn-1+MAXPRNGLO].iode != iode)
if (svr->nav.geph[prn - 1].iode != iode &&
svr->nav.geph[prn - 1 + MAXPRNGLO].iode != iode)
{
continue;
}
@@ -302,14 +303,14 @@ int decoderaw(rtksvr_t *svr, int index)
/* input rtcm/receiver raw data from stream */
if (svr->format[index] == STRFMT_RTCM2)
{
ret = input_rtcm2(svr->rtcm+index, svr->buff[index][i]);
ret = input_rtcm2(svr->rtcm + index, svr->buff[index][i]);
obs = &svr->rtcm[index].obs;
nav = &svr->rtcm[index].nav;
sat = svr->rtcm[index].ephsat;
}
else if (svr->format[index] == STRFMT_RTCM3)
{
ret = input_rtcm3(svr->rtcm+index, svr->buff[index][i]);
ret = input_rtcm3(svr->rtcm + index, svr->buff[index][i]);
obs = &svr->rtcm[index].obs;
nav = &svr->rtcm[index].nav;
sat = svr->rtcm[index].ephsat;
@@ -336,7 +337,10 @@ int decoderaw(rtksvr_t *svr, int index)
/* observation data received */
if (ret == 1)
{
if (fobs < MAXOBSBUF) fobs++; else svr->prcout++;
if (fobs < MAXOBSBUF)
fobs++;
else
svr->prcout++;
}
}
svr->nb[index] = 0;
@@ -350,30 +354,30 @@ int decoderaw(rtksvr_t *svr, int index)
/* decode download file ------------------------------------------------------*/
void decodefile(rtksvr_t *svr, int index)
{
int i = 0;
char glo_fcn[MAXPRNGLO+1];
int i = 0;
char glo_fcn[MAXPRNGLO + 1];
// Allocate space for GLONASS frequency channels depending on availability
for(i=0; i < MAXPRNGLO+1; i++)
glo_fcn[i]='0';
pcv_t pcvt0[MAXSAT] = { {0, {'0'}, {'0'}, {0, 0.0}, {0, 0.0}, {{0.0},{0.0}}, {{0.0},{0.0}} } };
// Allocate space for GLONASS frequency channels depending on availability
for (i = 0; i < MAXPRNGLO + 1; i++)
glo_fcn[i] = '0';
pcv_t pcvt0[MAXSAT] = {{0, {'0'}, {'0'}, {0, 0.0}, {0, 0.0}, {{0.0}, {0.0}}, {{0.0}, {0.0}}}};
sbsfcorr_t sbsfcorr0 = {{0, 0.0}, 0.0, 0.0, 0.0, 0, 0, 0};
sbslcorr_t sbslcorr0 = { {0, 0.0}, 0, {0.0}, {0.0}, 0.0, 0.0};
sbssat_t sbssat0 = {0, 0, 0, { {0, sbsfcorr0, sbslcorr0 } }};
sbsigp_t sbsigp0[MAXNIGP] = {{{0, 0.0}, 0, 0, 0, 0.0 }};
sbsion_t sbsion0[MAXBAND+1] = {{0, 0, {*sbsigp0} }};
dgps_t dgps0[MAXSAT] = { {{0, 0.0}, 0.0, 0.0, 0, 0.0 }};
ssr_t ssr0[MAXSAT] = {{ {{0, 0.0}}, {0.0}, {0}, 0, 0, 0, 0, {0.0}, {0.0}, {0.0}, 0.0, {0.0}, {0.0}, {0.0}, 0.0, 0.0, '0' }};
lexeph_t lexeph0[MAXSAT] = {{ {0,0.0}, {0,0.0}, 0, 0, 0, {0.0}, {0.0}, {0.0}, {0.0}, 0.0, 0.0, 0.0, {0.0} }};
stec_t stec0[MAXSTA] = {{ {0,0.0}, 0, 0.0, 0.0, {0.0}, 0}};
trop_t trop0[MAXSTA] = {{ {0, 0.0}, {0.0}, {0.0}}};
pppcorr_t pppcorr0 = {0, {{0},{0}}, {{0.0},{0.0}}, {0}, {0}, {0}, {0}, {stec0}, {trop0} };
sbslcorr_t sbslcorr0 = {{0, 0.0}, 0, {0.0}, {0.0}, 0.0, 0.0};
sbssat_t sbssat0 = {0, 0, 0, {{0, sbsfcorr0, sbslcorr0}}};
sbsigp_t sbsigp0[MAXNIGP] = {{{0, 0.0}, 0, 0, 0, 0.0}};
sbsion_t sbsion0[MAXBAND + 1] = {{0, 0, {*sbsigp0}}};
dgps_t dgps0[MAXSAT] = {{{0, 0.0}, 0.0, 0.0, 0, 0.0}};
ssr_t ssr0[MAXSAT] = {{{{0, 0.0}}, {0.0}, {0}, 0, 0, 0, 0, {0.0}, {0.0}, {0.0}, 0.0, {0.0}, {0.0}, {0.0}, 0.0, 0.0, '0'}};
lexeph_t lexeph0[MAXSAT] = {{{0, 0.0}, {0, 0.0}, 0, 0, 0, {0.0}, {0.0}, {0.0}, {0.0}, 0.0, 0.0, 0.0, {0.0}}};
stec_t stec0[MAXSTA] = {{{0, 0.0}, 0, 0.0, 0.0, {0.0}, 0}};
trop_t trop0[MAXSTA] = {{{0, 0.0}, {0.0}, {0.0}}};
pppcorr_t pppcorr0 = {0, {{0}, {0}}, {{0.0}, {0.0}}, {0}, {0}, {0}, {0}, {stec0}, {trop0}};
nav_t nav = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
{0, 0, (erpd_t *){0}}, {0.0}, {0.0}, {0.0}, {0.0}, {0.0}, {0.0}, {0.0}, {0.0},
{0.0}, {0.0}, {0.0}, {0.0}, 0, {{0.0},{0.0}}, {{0.0},{0.0}}, {{0.0}, {0.0}, {0.0}},
{0.0}, {0.0}, {*glo_fcn}, {*pcvt0}, sbssat0, {*sbsion0}, {*dgps0}, {*ssr0}, {*lexeph0},
{{0,0.0}, 0.0, {0.0}, {{0.0},{0.0}} }, pppcorr0} ;
{0, 0, (erpd_t *){0}}, {0.0}, {0.0}, {0.0}, {0.0}, {0.0}, {0.0}, {0.0}, {0.0},
{0.0}, {0.0}, {0.0}, {0.0}, 0, {{0.0}, {0.0}}, {{0.0}, {0.0}}, {{0.0}, {0.0}, {0.0}},
{0.0}, {0.0}, {*glo_fcn}, {*pcvt0}, sbssat0, {*sbsion0}, {*dgps0}, {*ssr0}, {*lexeph0},
{{0, 0.0}, 0.0, {0.0}, {{0.0}, {0.0}}}, pppcorr0};
char file[1024];
int nb;
@@ -384,17 +388,19 @@ void decodefile(rtksvr_t *svr, int index)
/* check file path completed */
if ((nb = svr->nb[index]) <= 2 ||
svr->buff[index][nb-2] != '\r' || svr->buff[index][nb-1] != '\n')
svr->buff[index][nb - 2] != '\r' || svr->buff[index][nb - 1] != '\n')
{
rtksvrunlock(svr);
return;
}
strncpy(file, (char *)svr->buff[index], nb-2); file[nb-2] = '\0';
strncpy(file, (char *)svr->buff[index], nb - 2);
file[nb - 2] = '\0';
svr->nb[index] = 0;
rtksvrunlock(svr);
if (svr->format[index] == STRFMT_SP3) { /* precise ephemeris */
if (svr->format[index] == STRFMT_SP3)
{ /* precise ephemeris */
/* read sp3 precise ephemeris */
readsp3(file, &nav, 0);
@@ -413,14 +419,16 @@ void decodefile(rtksvr_t *svr, int index)
strcpy(svr->files[index], file);
rtksvrunlock(svr);
}
else if (svr->format[index] == STRFMT_RNXCLK) { /* precise clock */
}
else if (svr->format[index] == STRFMT_RNXCLK)
{ /* precise clock */
/* read rinex clock */ // Disabled!!
if ( 1 /*readrnxc(file, &nav)<=0 */) {
/* read rinex clock */ // Disabled!!
if (1 /*readrnxc(file, &nav)<=0 */)
{
tracet(1, "rinex clock file read error: %s\n", file);
return;
}
}
/* update precise clock */
rtksvrlock(svr);
@@ -431,7 +439,7 @@ void decodefile(rtksvr_t *svr, int index)
strcpy(svr->files[index], file);
rtksvrunlock(svr);
}
}
}
@@ -440,7 +448,7 @@ void *rtksvrthread(void *arg)
{
rtksvr_t *svr = (rtksvr_t *)arg;
obs_t obs;
obsd_t data[MAXOBS*2];
obsd_t data[MAXOBS * 2];
double tt;
unsigned int tick, ticknmea;
unsigned char *p, *q;
@@ -448,31 +456,33 @@ void *rtksvrthread(void *arg)
tracet(3, "rtksvrthread:\n");
svr->state = 1; obs.data = data;
svr->state = 1;
obs.data = data;
svr->tick = tickget();
ticknmea = svr->tick-1000;
ticknmea = svr->tick - 1000;
for (cycle = 0;svr->state;cycle++)
for (cycle = 0; svr->state; cycle++)
{
tick = tickget();
for (i = 0; i < 3; i++)
{
p = svr->buff[i]+svr->nb[i]; q = svr->buff[i]+svr->buffsize;
p = svr->buff[i] + svr->nb[i];
q = svr->buff[i] + svr->buffsize;
/* read receiver raw/rtcm data from input stream */
if ((n = strread(svr->stream+i, p, q-p)) <= 0)
if ((n = strread(svr->stream + i, p, q - p)) <= 0)
{
continue;
}
/* write receiver raw/rtcm data to log stream */
strwrite(svr->stream+i+5, p, n);
strwrite(svr->stream + i + 5, p, n);
svr->nb[i] += n;
/* save peek buffer */
rtksvrlock(svr);
n = n < svr->buffsize-svr->npb[i] ? n : svr->buffsize-svr->npb[i];
memcpy(svr->pbuf[i]+svr->npb[i], p, n);
n = n < svr->buffsize - svr->npb[i] ? n : svr->buffsize - svr->npb[i];
memcpy(svr->pbuf[i] + svr->npb[i], p, n);
svr->npb[i] += n;
rtksvrunlock(svr);
}
@@ -492,11 +502,11 @@ void *rtksvrthread(void *arg)
for (i = 0; i < fobs[0]; i++)
{ /* for each rover observation data */
obs.n = 0;
for (j = 0; j < svr->obs[0][i].n && obs.n < MAXOBS*2; j++)
for (j = 0; j < svr->obs[0][i].n && obs.n < MAXOBS * 2; j++)
{
obs.data[obs.n++] = svr->obs[0][i].data[j];
}
for (j = 0; j < svr->obs[1][0].n && obs.n < MAXOBS*2; j++)
for (j = 0; j < svr->obs[1][0].n && obs.n < MAXOBS * 2; j++)
{
obs.data[obs.n++] = svr->obs[1][0].data[j];
}
@@ -508,60 +518,63 @@ void *rtksvrthread(void *arg)
if (svr->rtk.sol.stat != SOLQ_NONE)
{
/* adjust current time */
tt = (int)(tickget()-tick)/1000.0+DTTOL;
tt = (int)(tickget() - tick) / 1000.0 + DTTOL;
timeset(gpst2utc(timeadd(svr->rtk.sol.time, tt)));
/* write solution */
writesol(svr, i);
}
/* if cpu overload, inclement obs outage counter and break */
if ((int)(tickget()-tick) >= svr->cycle)
if ((int)(tickget() - tick) >= svr->cycle)
{
svr->prcout += fobs[0]-i-1;
svr->prcout += fobs[0] - i - 1;
#if 0 /* omitted v.2.4.1 */
break;
#endif
}
}
/* send null solution if no solution (1hz) */
if (svr->rtk.sol.stat == SOLQ_NONE && cycle%(1000/svr->cycle) == 0)
if (svr->rtk.sol.stat == SOLQ_NONE && cycle % (1000 / svr->cycle) == 0)
{
writesol(svr, 0);
}
/* send nmea request to base/nrtk input stream */
if (svr->nmeacycle>0 && (int)(tick-ticknmea) >= svr->nmeacycle)
if (svr->nmeacycle > 0 && (int)(tick - ticknmea) >= svr->nmeacycle)
{
if (svr->stream[1].state == 1)
{
if (svr->nmeareq == 1)
{
strsendnmea(svr->stream+1, svr->nmeapos);
strsendnmea(svr->stream + 1, svr->nmeapos);
}
else if (svr->nmeareq == 2 && norm_rtk(svr->rtk.sol.rr, 3)>0.0)
else if (svr->nmeareq == 2 && norm_rtk(svr->rtk.sol.rr, 3) > 0.0)
{
strsendnmea(svr->stream+1, svr->rtk.sol.rr);
strsendnmea(svr->stream + 1, svr->rtk.sol.rr);
}
}
ticknmea = tick;
}
if ((cputime = (int)(tickget()-tick))>0) svr->cputime = cputime;
if ((cputime = (int)(tickget() - tick)) > 0) svr->cputime = cputime;
/* sleep until next cycle */
sleepms(svr->cycle-cputime);
sleepms(svr->cycle - cputime);
}
for (i = 0; i < MAXSTRRTK; i++) strclose(svr->stream+i);
for (i = 0; i < MAXSTRRTK; i++) strclose(svr->stream + i);
for (i = 0; i < 3; i++)
{
svr->nb[i] = svr->npb[i] = 0;
free(svr->buff[i]); svr->buff[i] = NULL;
free(svr->pbuf[i]); svr->pbuf[i] = NULL;
free(svr->buff[i]);
svr->buff[i] = NULL;
free(svr->pbuf[i]);
svr->pbuf[i] = NULL;
//free_raw (svr->raw +i);
free_rtcm(svr->rtcm+i);
free_rtcm(svr->rtcm + i);
}
for (i = 0; i < 2; i++)
{
svr->nsb[i] = 0;
free(svr->sbuf[i]); svr->sbuf[i] = NULL;
free(svr->sbuf[i]);
svr->sbuf[i] = NULL;
}
return 0;
}
@@ -575,14 +588,14 @@ void *rtksvrthread(void *arg)
int rtksvrinit(rtksvr_t *svr)
{
gtime_t time0 = {0, 0.0};
sol_t sol0 = {{0,0}, {0,0,0,0,0,0}, {0,0,0,0,0,0}, {0,0,0,0,0,0},
'0', '0', '0', 0, 0, 0 };
eph_t eph0 = {0, -1, -1, 0, 0, 0, 0, 0, {0,0.0}, {0,0.0}, {0,0.0},
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, 0.0, 0.0};
geph_t geph0 = {0, -1, 0, 0, 0, 0, {0,0.0}, {0,0.0}, {0.0}, {0.0}, {0.0},
0.0, 0.0, 0.0};
seph_t seph0 = {0, {0,0.0}, {0,0.0}, 0, 0, {0.0}, {0.0}, {0.0}, 0.0, 0.0};
sol_t sol0 = {{0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
'0', '0', '0', 0, 0, 0};
eph_t eph0 = {0, -1, -1, 0, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0, 0.0},
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, 0.0, 0.0};
geph_t geph0 = {0, -1, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0.0}, {0.0}, {0.0},
0.0, 0.0, 0.0};
seph_t seph0 = {0, {0, 0.0}, {0, 0.0}, 0, 0, {0.0}, {0.0}, {0.0}, 0.0, 0.0};
int i, j;
tracet(3, "rtksvrinit:\n");
@@ -601,7 +614,8 @@ int rtksvrinit(rtksvr_t *svr)
for (i = 0; i < 2; i++) svr->sbuf[i] = NULL;
for (i = 0; i < 3; i++) svr->pbuf[i] = NULL;
for (i = 0; i < MAXSOLBUF; i++) svr->solbuf[i] = sol0;
for (i = 0; i < 3; i++) for (j = 0; j < 10; j++) svr->nmsg[i][j] = 0;
for (i = 0; i < 3; i++)
for (j = 0; j < 10; j++) svr->nmsg[i][j] = 0;
for (i = 0; i < 3; i++) svr->ftime[i] = time0;
for (i = 0; i < 3; i++) svr->files[i][0] = '\0';
svr->moni = NULL;
@@ -609,34 +623,35 @@ int rtksvrinit(rtksvr_t *svr)
svr->thread = 0;
svr->cputime = svr->prcout = 0;
if (!(svr->nav.eph = (eph_t *)malloc(sizeof(eph_t )*MAXSAT *2)) ||
!(svr->nav.geph = (geph_t *)malloc(sizeof(geph_t)*NSATGLO*2)) ||
!(svr->nav.seph = (seph_t *)malloc(sizeof(seph_t)*NSATSBS*2)))
if (!(svr->nav.eph = (eph_t *)malloc(sizeof(eph_t) * MAXSAT * 2)) ||
!(svr->nav.geph = (geph_t *)malloc(sizeof(geph_t) * NSATGLO * 2)) ||
!(svr->nav.seph = (seph_t *)malloc(sizeof(seph_t) * NSATSBS * 2)))
{
tracet(1, "rtksvrinit: malloc error\n");
return 0;
}
for (i = 0; i < MAXSAT *2; i++) svr->nav.eph [i] = eph0;
for (i = 0; i < NSATGLO*2; i++) svr->nav.geph[i] = geph0;
for (i = 0; i < NSATSBS*2; i++) svr->nav.seph[i] = seph0;
svr->nav.n = MAXSAT *2;
svr->nav.ng = NSATGLO*2;
svr->nav.ns = NSATSBS*2;
for (i = 0; i < MAXSAT * 2; i++) svr->nav.eph[i] = eph0;
for (i = 0; i < NSATGLO * 2; i++) svr->nav.geph[i] = geph0;
for (i = 0; i < NSATSBS * 2; i++) svr->nav.seph[i] = seph0;
svr->nav.n = MAXSAT * 2;
svr->nav.ng = NSATGLO * 2;
svr->nav.ns = NSATSBS * 2;
for (i = 0; i < 3; i++) for (j = 0; j < MAXOBSBUF; j++)
{
if (!(svr->obs[i][j].data = (obsd_t *)malloc(sizeof(obsd_t)*MAXOBS)))
{
tracet(1, "rtksvrinit: malloc error\n");
return 0;
}
}
for (i = 0; i < 3; i++)
for (j = 0; j < MAXOBSBUF; j++)
{
if (!(svr->obs[i][j].data = (obsd_t *)malloc(sizeof(obsd_t) * MAXOBS)))
{
tracet(1, "rtksvrinit: malloc error\n");
return 0;
}
}
for (i = 0; i < 3; i++)
{
memset(svr->raw +i, 0, sizeof(raw_t ));
memset(svr->rtcm+i, 0, sizeof(rtcm_t));
memset(svr->raw + i, 0, sizeof(raw_t));
memset(svr->rtcm + i, 0, sizeof(rtcm_t));
}
for (i = 0; i < MAXSTRRTK; i++) strinit(svr->stream+i);
for (i = 0; i < MAXSTRRTK; i++) strinit(svr->stream + i);
initlock(&svr->lock);
@@ -653,13 +668,14 @@ void rtksvrfree(rtksvr_t *svr)
{
int i, j;
free(svr->nav.eph );
free(svr->nav.eph);
free(svr->nav.geph);
free(svr->nav.seph);
for (i = 0; i < 3; i++) for (j = 0; j < MAXOBSBUF; j++)
{
free(svr->obs[i][j].data);
}
for (i = 0; i < 3; i++)
for (j = 0; j < MAXOBSBUF; j++)
{
free(svr->obs[i][j].data);
}
}
@@ -668,10 +684,10 @@ void rtksvrfree(rtksvr_t *svr)
* args : rtksvr_t *svr IO rtk server
* return : status (1:ok 0:error)
*-----------------------------------------------------------------------------*/
void rtksvrlock (rtksvr_t *svr) {rtk_lock (&svr->lock);}
void rtksvrlock(rtksvr_t *svr) { rtk_lock(&svr->lock); }
void rtksvrunlock(rtksvr_t *svr) {rtk_unlock(&svr->lock);}
void rtksvrunlock(rtksvr_t *svr) { rtk_unlock(&svr->lock); }
/* start rtk server ------------------------------------------------------------
@@ -714,16 +730,16 @@ void rtksvrunlock(rtksvr_t *svr) {rtk_unlock(&svr->lock);}
* return : status (1:ok 0:error)
*-----------------------------------------------------------------------------*/
int rtksvrstart(rtksvr_t *svr, int cycle, int buffsize, int *strs,
char **paths, int *formats, int navsel, char **cmds,
char **rcvopts, int nmeacycle, int nmeareq,
const double *nmeapos, prcopt_t *prcopt,
solopt_t *solopt, stream_t *moni)
char **paths, int *formats, int navsel, char **cmds,
char **rcvopts, int nmeacycle, int nmeareq,
const double *nmeapos, prcopt_t *prcopt,
solopt_t *solopt, stream_t *moni)
{
gtime_t time, time0 = {0, 0.0};
int i, j, rw;
tracet(3, "rtksvrstart: cycle=%d buffsize=%d navsel=%d nmeacycle=%d nmeareq=%d\n",
cycle, buffsize, navsel, nmeacycle, nmeareq);
cycle, buffsize, navsel, nmeacycle, nmeareq);
if (svr->state) return 0;
@@ -745,7 +761,7 @@ int rtksvrstart(rtksvr_t *svr, int cycle, int buffsize, int *strs,
{ /* input/log streams */
svr->nb[i] = svr->npb[i] = 0;
if (!(svr->buff[i] = (unsigned char *)malloc(buffsize)) ||
!(svr->pbuf[i] = (unsigned char *)malloc(buffsize)))
!(svr->pbuf[i] = (unsigned char *)malloc(buffsize)))
{
tracet(1, "rtksvrstart: malloc error\n");
return 0;
@@ -755,11 +771,11 @@ int rtksvrstart(rtksvr_t *svr, int cycle, int buffsize, int *strs,
/* initialize receiver raw and rtcm control */
//init_raw (svr->raw +i);
init_rtcm(svr->rtcm+i);
init_rtcm(svr->rtcm + i);
/* set receiver and rtcm option */
if(strlen(rcvopts[i]) < 256 ) strcpy(svr->raw [i].opt, rcvopts[i]);
if(strlen(rcvopts[i]) < 256 ) strcpy(svr->rtcm[i].opt, rcvopts[i]);
if (strlen(rcvopts[i]) < 256) strcpy(svr->raw[i].opt, rcvopts[i]);
if (strlen(rcvopts[i]) < 256) strcpy(svr->rtcm[i].opt, rcvopts[i]);
/* connect dgps corrections */
svr->rtcm[i].dgps = svr->nav.dgps;
@@ -783,9 +799,9 @@ int rtksvrstart(rtksvr_t *svr, int cycle, int buffsize, int *strs,
svr->rtk.rb[i] = i < 3 ? prcopt->rb[i] : 0.0;
}
/* update navigation data */
for (i = 0; i < MAXSAT *2; i++) svr->nav.eph [i].ttr = time0;
for (i = 0; i < NSATGLO*2; i++) svr->nav.geph[i].tof = time0;
for (i = 0; i < NSATSBS*2; i++) svr->nav.seph[i].tof = time0;
for (i = 0; i < MAXSAT * 2; i++) svr->nav.eph[i].ttr = time0;
for (i = 0; i < NSATGLO * 2; i++) svr->nav.geph[i].tof = time0;
for (i = 0; i < NSATSBS * 2; i++) svr->nav.seph[i].tof = time0;
updatenav(&svr->nav);
/* set monitor stream */
@@ -796,37 +812,37 @@ int rtksvrstart(rtksvr_t *svr, int cycle, int buffsize, int *strs,
{
rw = i < 3 ? STR_MODE_R : STR_MODE_W;
if (strs[i] != STR_FILE) rw |= STR_MODE_W;
if (!stropen(svr->stream+i, strs[i], rw, paths[i]))
if (!stropen(svr->stream + i, strs[i], rw, paths[i]))
{
for (i--; i >= 0; i--) strclose(svr->stream+i);
for (i--; i >= 0; i--) strclose(svr->stream + i);
return 0;
}
/* set initial time for rtcm and raw */
if (i < 3)
{
time = utc2gpst(timeget());
svr->raw [i].time = strs[i] == STR_FILE ? strgettime(svr->stream+i) : time;
svr->rtcm[i].time = strs[i] == STR_FILE ? strgettime(svr->stream+i) : time;
svr->raw[i].time = strs[i] == STR_FILE ? strgettime(svr->stream + i) : time;
svr->rtcm[i].time = strs[i] == STR_FILE ? strgettime(svr->stream + i) : time;
}
}
/* sync input streams */
strsync(svr->stream, svr->stream+1);
strsync(svr->stream, svr->stream+2);
strsync(svr->stream, svr->stream + 1);
strsync(svr->stream, svr->stream + 2);
/* write start commands to input streams */
for (i = 0; i < 3; i++)
{
if (cmds[i]) strsendcmd(svr->stream+i, cmds[i]);
if (cmds[i]) strsendcmd(svr->stream + i, cmds[i]);
}
/* write solution header to solution streams */
for (i = 3; i < 5; i++)
{
writesolhead(svr->stream+i, svr->solopt+i-3);
writesolhead(svr->stream + i, svr->solopt + i - 3);
}
/* create rtk server thread */
if (pthread_create(&svr->thread, NULL, rtksvrthread, svr))
{
for (i = 0; i < MAXSTRRTK; i++) strclose(svr->stream+i);
for (i = 0; i < MAXSTRRTK; i++) strclose(svr->stream + i);
return 0;
}
return 1;
@@ -852,7 +868,7 @@ void rtksvrstop(rtksvr_t *svr, char **cmds)
rtksvrlock(svr);
for (i = 0; i < 3; i++)
{
if (cmds[i]) strsendcmd(svr->stream+i, cmds[i]);
if (cmds[i]) strsendcmd(svr->stream + i, cmds[i]);
}
rtksvrunlock(svr);
@@ -876,7 +892,7 @@ void rtksvrstop(rtksvr_t *svr, char **cmds)
* return : status (1:ok 0:error)
*-----------------------------------------------------------------------------*/
int rtksvropenstr(rtksvr_t *svr, int index, int str, const char *path,
const solopt_t *solopt)
const solopt_t *solopt)
{
tracet(3, "rtksvropenstr: index=%d str=%d path=%s\n", index, str, path);
@@ -884,12 +900,12 @@ int rtksvropenstr(rtksvr_t *svr, int index, int str, const char *path,
rtksvrlock(svr);
if (svr->stream[index].state>0)
if (svr->stream[index].state > 0)
{
rtksvrunlock(svr);
return 0;
}
if (!stropen(svr->stream+index, str, STR_MODE_W, path))
if (!stropen(svr->stream + index, str, STR_MODE_W, path))
{
tracet(2, "stream open error: index=%d\n", index);
rtksvrunlock(svr);
@@ -897,10 +913,10 @@ int rtksvropenstr(rtksvr_t *svr, int index, int str, const char *path,
}
if (index <= 4)
{
svr->solopt[index-3] = *solopt;
svr->solopt[index - 3] = *solopt;
/* write solution header to solution stream */
writesolhead(svr->stream+index, svr->solopt+index-3);
writesolhead(svr->stream + index, svr->solopt + index - 3);
}
rtksvrunlock(svr);
return 1;
@@ -919,11 +935,11 @@ void rtksvrclosestr(rtksvr_t *svr, int index)
{
tracet(3, "rtksvrclosestr: index=%d\n", index);
if (index < 3 || index>7 || !svr->state) return;
if (index < 3 || index > 7 || !svr->state) return;
rtksvrlock(svr);
strclose(svr->stream+index);
strclose(svr->stream + index);
rtksvrunlock(svr);
}
@@ -943,7 +959,7 @@ void rtksvrclosestr(rtksvr_t *svr, int index)
* return : number of satellites
*-----------------------------------------------------------------------------*/
int rtksvrostat(rtksvr_t *svr, int rcv, gtime_t *time, int *sat,
double *az, double *el, int **snr, int *vsat)
double *az, double *el, int **snr, int *vsat)
{
int i, j, ns;
@@ -952,26 +968,26 @@ int rtksvrostat(rtksvr_t *svr, int rcv, gtime_t *time, int *sat,
if (!svr->state) return 0;
rtksvrlock(svr);
ns = svr->obs[rcv][0].n;
if (ns>0)
if (ns > 0)
{
*time = svr->obs[rcv][0].data[0].time;
}
for (i = 0; i < ns; i++)
{
sat [i] = svr->obs[rcv][0].data[i].sat;
az [i] = svr->rtk.ssat[sat[i]-1].azel[0];
el [i] = svr->rtk.ssat[sat[i]-1].azel[1];
sat[i] = svr->obs[rcv][0].data[i].sat;
az[i] = svr->rtk.ssat[sat[i] - 1].azel[0];
el[i] = svr->rtk.ssat[sat[i] - 1].azel[1];
for (j = 0; j < NFREQ; j++)
{
snr[i][j] = (int)(svr->obs[rcv][0].data[i].SNR[j]*0.25);
snr[i][j] = (int)(svr->obs[rcv][0].data[i].SNR[j] * 0.25);
}
if (svr->rtk.sol.stat == SOLQ_NONE || svr->rtk.sol.stat == SOLQ_SINGLE)
if (svr->rtk.sol.stat == SOLQ_NONE || svr->rtk.sol.stat == SOLQ_SINGLE)
{
vsat[i] = svr->rtk.ssat[sat[i]-1].vs;
vsat[i] = svr->rtk.ssat[sat[i] - 1].vs;
}
else
{
vsat[i] = svr->rtk.ssat[sat[i]-1].vsat[0];
vsat[i] = svr->rtk.ssat[sat[i] - 1].vsat[0];
}
}
rtksvrunlock(svr);
@@ -996,8 +1012,8 @@ void rtksvrsstat(rtksvr_t *svr, int *sstat, char *msg)
rtksvrlock(svr);
for (i = 0; i < MAXSTRRTK; i++)
{
sstat[i] = strstat(svr->stream+i, s);
if (*s) p += sprintf(p, "(%d) %s ", i+1, s);
sstat[i] = strstat(svr->stream + i, s);
if (*s) p += sprintf(p, "(%d) %s ", i + 1, s);
}
rtksvrunlock(svr);
}

71
src/algorithms/libs/rtklib/rtklib_rtksvr.h
View File

@@ -57,35 +57,35 @@
#include "rtklib.h"
const solopt_t solopt_default = { /* defaults solution output options */
SOLF_LLH, TIMES_GPST, 1, 3, /* posf, times, timef, timeu */
0, 1, 0, 0, 0, 0, /* degf, outhead, outopt, datum, height, geoid */
0, 0, 0, /* solstatic, sstat, trace */
{0.0, 0.0}, /* nmeaintv */
" ", "", 0 /* separator/program name */
const solopt_t solopt_default = {
/* defaults solution output options */
SOLF_LLH, TIMES_GPST, 1, 3, /* posf, times, timef, timeu */
0, 1, 0, 0, 0, 0, /* degf, outhead, outopt, datum, height, geoid */
0, 0, 0, /* solstatic, sstat, trace */
{0.0, 0.0}, /* nmeaintv */
" ", "", 0 /* separator/program name */
};
const prcopt_t prcopt_default = { /* defaults processing options */
PMODE_SINGLE, 0, 2, SYS_GPS, /* mode, soltype, nf, navsys */
15.0*D2R, { {}, {{},{}} }, /* elmin, snrmask */
0, 1, 1, 1, /* sateph, modear, glomodear, bdsmodear */
5, 0, 10, 1, /* maxout, minlock, minfix, armaxiter */
0, 0, 0, 0, /* estion, esttrop, dynamics, tidecorr */
1, 0, 0, 0, 0, /* niter, codesmooth, intpref, sbascorr, sbassatsel */
0, 0, /* rovpos, refpos */
{100.0, 100.0, 100.0}, /* eratio[] */
{100.0, 0.003, 0.003, 0.0, 1.0}, /* err[] */
{30.0, 0.03, 0.3}, /* std[] */
{1e-4, 1e-3, 1e-4, 1e-1, 1e-2, 0.0}, /* prn[] */
5E-12, /* sclkstab */
{3.0, 0.9999, 0.25, 0.1, 0.05, 0, 0, 0}, /* thresar */
0.0, 0.0, 0.05, /* elmaskar, almaskhold, thresslip */
30.0, 30.0, 30.0, /* maxtdif, maxinno, maxgdop */
{}, {}, {}, /* baseline, ru, rb */
{"",""}, /* anttype */
{} , {}, {}, /* antdel, pcv, exsats */
0, 0, 0, {"",""}, {}, 0, {{},{}}, { {}, {{},{}}, {{},{}}, {}, {} }, 0, {}
};
const prcopt_t prcopt_default = { /* defaults processing options */
PMODE_SINGLE, 0, 2, SYS_GPS, /* mode, soltype, nf, navsys */
15.0 * D2R, {{}, {{}, {}}}, /* elmin, snrmask */
0, 1, 1, 1, /* sateph, modear, glomodear, bdsmodear */
5, 0, 10, 1, /* maxout, minlock, minfix, armaxiter */
0, 0, 0, 0, /* estion, esttrop, dynamics, tidecorr */
1, 0, 0, 0, 0, /* niter, codesmooth, intpref, sbascorr, sbassatsel */
0, 0, /* rovpos, refpos */
{100.0, 100.0, 100.0}, /* eratio[] */
{100.0, 0.003, 0.003, 0.0, 1.0}, /* err[] */
{30.0, 0.03, 0.3}, /* std[] */
{1e-4, 1e-3, 1e-4, 1e-1, 1e-2, 0.0}, /* prn[] */
5E-12, /* sclkstab */
{3.0, 0.9999, 0.25, 0.1, 0.05, 0, 0, 0}, /* thresar */
0.0, 0.0, 0.05, /* elmaskar, almaskhold, thresslip */
30.0, 30.0, 30.0, /* maxtdif, maxinno, maxgdop */
{}, {}, {}, /* baseline, ru, rb */
{"", ""}, /* anttype */
{}, {}, {}, /* antdel, pcv, exsats */
0, 0, 0, {"", ""}, {}, 0, {{}, {}}, {{}, {{}, {}}, {{}, {}}, {}, {}}, 0, {}};
void writesolhead(stream_t *stream, const solopt_t *solopt);
@@ -99,7 +99,7 @@ void updatenav(nav_t *nav);
void updatefcn(rtksvr_t *svr);
void updatesvr(rtksvr_t *svr, int ret, obs_t *obs, nav_t *nav, int sat,
sbsmsg_t *sbsmsg, int index, int iobs);
sbsmsg_t *sbsmsg, int index, int iobs);
int decoderaw(rtksvr_t *svr, int index);
@@ -111,28 +111,27 @@ int rtksvrinit(rtksvr_t *svr);
void rtksvrfree(rtksvr_t *svr);
void rtksvrlock (rtksvr_t *svr);
void rtksvrlock(rtksvr_t *svr);
void rtksvrunlock(rtksvr_t *svr);
int rtksvrstart(rtksvr_t *svr, int cycle, int buffsize, int *strs,
char **paths, int *formats, int navsel, char **cmds,
char **rcvopts, int nmeacycle, int nmeareq,
const double *nmeapos, prcopt_t *prcopt,
solopt_t *solopt, stream_t *moni);
char **paths, int *formats, int navsel, char **cmds,
char **rcvopts, int nmeacycle, int nmeareq,
const double *nmeapos, prcopt_t *prcopt,
solopt_t *solopt, stream_t *moni);
void rtksvrstop(rtksvr_t *svr, char **cmds);
int rtksvropenstr(rtksvr_t *svr, int index, int str, const char *path,
const solopt_t *solopt);
const solopt_t *solopt);
void rtksvrclosestr(rtksvr_t *svr, int index);
int rtksvrostat(rtksvr_t *svr, int rcv, gtime_t *time, int *sat,
double *az, double *el, int **snr, int *vsat);
double *az, double *el, int **snr, int *vsat);
void rtksvrsstat(rtksvr_t *svr, int *sstat, char *msg);
#endif

518
src/algorithms/libs/rtklib/rtklib_sbas.cc
View File

@@ -65,7 +65,8 @@
/* extract field from line ---------------------------------------------------*/
char *getfield(char *p, int pos)
{
for (pos--; pos > 0 ; pos--, p++) if (!(p = strchr(p, ','))) return NULL;
for (pos--; pos > 0; pos--, p++)
if (!(p = strchr(p, ','))) return NULL;
return p;
}
@@ -74,10 +75,9 @@ char *getfield(char *p, int pos)
double varfcorr(int udre)
{
const double var[14] = {
0.052, 0.0924, 0.1444, 0.283, 0.4678, 0.8315, 1.2992, 1.8709, 2.5465, 3.326,
5.1968, 20.7870, 230.9661, 2078.695
};
return 0 < udre && udre <= 14 ? var[udre-1] : 0.0;
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;
}
@@ -85,10 +85,9 @@ double varfcorr(int udre)
double varicorr(int give)
{
const double var[15] = {
0.0084, 0.0333, 0.0749, 0.1331, 0.2079, 0.2994, 0.4075, 0.5322, 0.6735, 0.8315,
1.1974, 1.8709, 3.326, 20.787, 187.0826
};
return 0 < give && give <= 15 ? var[give-1] : 0.0;
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 - 1] : 0.0;
}
@@ -96,9 +95,8 @@ double varicorr(int give)
double 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;
}
@@ -110,18 +108,26 @@ int decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat)
trace(4, "decode_sbstype1:\n");
for (i = 1, n = 0;i<=210 && n<MAXSAT;i++)
for (i = 1, n = 0; i <= 210 && n < MAXSAT; i++)
{
if (getbitu(msg->msg, 13+i, 1))
if (getbitu(msg->msg, 13 + i, 1))
{
if (i<= 37) sat = satno(SYS_GPS, i); /* 0- 37: gps */
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 */
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 */
sbssat->sat[n++].sat = sat;
}
}
@@ -142,29 +148,29 @@ int decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat)
trace(4, "decode_sbstype2:\n");
if (sbssat->iodp!=(int)getbitu(msg->msg, 16, 2)) return 0;
if (sbssat->iodp != (int)getbitu(msg->msg, 16, 2)) return 0;
type = getbitu(msg->msg, 8, 6);
type = getbitu(msg->msg, 8, 6);
iodf = getbitu(msg->msg, 14, 2);
for (i = 0; i < 13; i++)
{
if ((j = 13 * ((type == 0 ? 2 : type) - 2) + i) >= sbssat->nsat) break;
udre = getbitu(msg->msg, 174+4*i, 4);
t0 = sbssat->sat[j].fcorr.t0;
udre = getbitu(msg->msg, 174 + 4 * i, 4);
t0 = sbssat->sat[j].fcorr.t0;
prc = sbssat->sat[j].fcorr.prc;
sbssat->sat[j].fcorr.t0 = gpst2time(msg->week, msg->tow);
sbssat->sat[j].fcorr.prc = getbits(msg->msg, 18 + i * 12, 12) * 0.125f;
sbssat->sat[j].fcorr.udre = udre + 1;
dt = timediff(sbssat->sat[j].fcorr.t0, t0);
if (t0.time == 0||dt <= 0.0 || 18.0 < dt || sbssat->sat[j].fcorr.ai == 0)
if (t0.time == 0 || dt <= 0.0 || 18.0 < dt || sbssat->sat[j].fcorr.ai == 0)
{
sbssat->sat[j].fcorr.rrc = 0.0;
sbssat->sat[j].fcorr.dt = 0.0;
}
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.iodf = iodf;
@@ -181,15 +187,15 @@ int decode_sbstype6(const sbsmsg_t *msg, sbssat_t *sbssat)
trace(4, "decode_sbstype6:\n");
for (i = 0;i<4;i++)
for (i = 0; i < 4; i++)
{
iodf[i] = getbitu(msg->msg, 14+i*2, 2);
iodf[i] = getbitu(msg->msg, 14 + i * 2, 2);
}
for (i = 0; i < sbssat->nsat && i < MAXSAT; i++)
{
if (sbssat->sat[i].fcorr.iodf != iodf[i/28]) continue;
udre = getbitu(msg->msg, 22+i*4, 4);
sbssat->sat[i].fcorr.udre = udre+1;
if (sbssat->sat[i].fcorr.iodf != iodf[i / 28]) continue;
udre = getbitu(msg->msg, 22 + i * 4, 4);
sbssat->sat[i].fcorr.udre = udre + 1;
}
trace(5, "decode_sbstype6: iodf=%d %d %d %d\n", iodf[0], iodf[1], iodf[2], iodf[3]);
return 1;
@@ -203,13 +209,13 @@ int decode_sbstype7(const sbsmsg_t *msg, sbssat_t *sbssat)
trace(4, "decode_sbstype7\n");
if (sbssat->iodp!=(int)getbitu(msg->msg, 18, 2)) return 0;
if (sbssat->iodp != (int)getbitu(msg->msg, 18, 2)) return 0;
sbssat->tlat = getbitu(msg->msg, 14, 4);
for (i = 0;i<sbssat->nsat && i<MAXSAT;i++)
for (i = 0; i < sbssat->nsat && i < MAXSAT; i++)
{
sbssat->sat[i].fcorr.ai = getbitu(msg->msg, 22+i*4, 4);
sbssat->sat[i].fcorr.ai = getbitu(msg->msg, 22 + i * 4, 4);
}
return 1;
}
@@ -218,7 +224,7 @@ 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)
{
seph_t seph = { 0, {0,0}, {0,0}, 0, 0, {}, {}, {}, 0.0, 0.0 };
seph_t seph = {0, {0, 0}, {0, 0}, 0, 0, {}, {}, {}, 0.0, 0.0};
int i, sat, t;
trace(4, "decode_sbstype9:\n");
@@ -228,35 +234,37 @@ int decode_sbstype9(const sbsmsg_t *msg, nav_t *nav)
trace(2, "invalid prn in sbas type 9: prn=%3d\n", msg->prn);
return 0;
}
t = (int)getbitu(msg->msg, 22, 13)*16-(int)msg->tow%86400;
if (t<=-43200) t+=86400;
else if (t> 43200) t-=86400;
t = (int)getbitu(msg->msg, 22, 13) * 16 - (int)msg->tow % 86400;
if (t <= -43200)
t += 86400;
else if (t > 43200)
t -= 86400;
seph.sat = sat;
seph.t0 = gpst2time(msg->week, msg->tow+t);
seph.t0 = gpst2time(msg->week, msg->tow + t);
seph.tof = gpst2time(msg->week, msg->tow);
seph.sva = getbitu(msg->msg, 35, 4);
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)*TWO_N31;
seph.af1 = getbits(msg->msg, 218, 8)*TWO_N39/2.0;
seph.af0 = getbits(msg->msg, 206, 12) * TWO_N31;
seph.af1 = getbits(msg->msg, 218, 8) * TWO_N39 / 2.0;
i = msg->prn-MINPRNSBS;
if (!nav->seph||fabs(timediff(nav->seph[i].t0, seph.t0))<1e-3)
i = msg->prn - MINPRNSBS;
if (!nav->seph || fabs(timediff(nav->seph[i].t0, seph.t0)) < 1e-3)
{ /* not change */
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\n", msg->prn);
return 1;
@@ -271,15 +279,24 @@ int decode_sbstype18(const sbsmsg_t *msg, sbsion_t *sbsion)
trace(4, "decode_sbstype18:\n");
if (0<=band && band<= 8) {p = igpband1[band ]; m = 8;}
else if (9<=band && band<=10) {p = igpband2[band-9]; m = 5;}
else return 0;
if (0 <= band && band <= 8)
{
p = igpband1[band];
m = 8;
}
else if (9 <= band && band <= 10)
{
p = igpband2[band - 9];
m = 5;
}
else
return 0;
sbsion[band].iodi = (short)getbitu(msg->msg, 22, 2);
for (i = 1, n = 0; i<= 201;i++)
for (i = 1, n = 0; i <= 201; i++)
{
if (!getbitu(msg->msg , 23 + i, 1)) continue;
if (!getbitu(msg->msg, 23 + i, 1)) continue;
for (j = 0; j < m; j++)
{
if (i < p[j].bits || p[j].bite < i) continue;
@@ -302,20 +319,20 @@ int decode_longcorr0(const sbsmsg_t *msg, int p, sbssat_t *sbssat)
trace(4, "decode_longcorr0:\n");
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) * TWO_N31;
sbssat->sat[n-1].lcorr.daf1 = 0.0;
sbssat->sat[n-1].lcorr.t0 = gpst2time(msg->week, msg->tow);
sbssat->sat[n - 1].lcorr.daf0 = getbits(msg->msg, p + 41, 10) * TWO_N31;
sbssat->sat[n - 1].lcorr.daf1 = 0.0;
sbssat->sat[n - 1].lcorr.t0 = gpst2time(msg->week, msg->tow);
trace(5, "decode_longcorr0:sat=%2d\n", sbssat->sat[n-1].sat);
trace(5, "decode_longcorr0:sat=%2d\n", sbssat->sat[n - 1].sat);
return 1;
}
@@ -327,23 +344,25 @@ int decode_longcorr1(const sbsmsg_t *msg, int p, sbssat_t *sbssat)
trace(4, "decode_longcorr1:\n");
if (n == 0||n>MAXSAT) return 0;
if (n == 0 || n > MAXSAT) return 0;
sbssat->sat[n-1].lcorr.iode = getbitu(msg->msg, p+6, 8);
sbssat->sat[n - 1].lcorr.iode = getbitu(msg->msg, p + 6, 8);
for (i = 0;i<3;i++)
for (i = 0; i < 3; i++)
{
sbssat->sat[n-1].lcorr.dpos[i] = getbits(msg->msg, p + 14 + i * 11, 11) * 0.125;
sbssat->sat[n-1].lcorr.dvel[i] = getbits(msg->msg, p + 58 + i * 8, 8) * TWO_N11;
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) * TWO_N11;
}
sbssat->sat[n-1].lcorr.daf0 = getbits(msg->msg, p+47, 11) * TWO_N31;
sbssat->sat[n-1].lcorr.daf1 = getbits(msg->msg, p+82, 8) * TWO_N39;
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.daf0 = getbits(msg->msg, p + 47, 11) * TWO_N31;
sbssat->sat[n - 1].lcorr.daf1 = getbits(msg->msg, p + 82, 8) * TWO_N39;
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);
trace(5, "decode_longcorr1: sat=%2d\n", sbssat->sat[n-1].sat);
trace(5, "decode_longcorr1: sat=%2d\n", sbssat->sat[n - 1].sat);
return 1;
}
@@ -353,16 +372,17 @@ int decode_longcorrh(const sbsmsg_t *msg, int p, sbssat_t *sbssat)
{
trace(4, "decode_longcorrh:\n");
if (getbitu(msg->msg, p, 1) == 0) { /* vel code=0 */
if (sbssat->iodp == (int)getbitu(msg->msg, p+103, 2))
if (getbitu(msg->msg, p, 1) == 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))
}
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;
}
@@ -375,19 +395,19 @@ int decode_sbstype24(const sbsmsg_t *msg, sbssat_t *sbssat)
trace(4, "decode_sbstype24:\n");
if (sbssat->iodp!=(int)getbitu(msg->msg, 110, 2)) return 0; /* check IODP */
if (sbssat->iodp != (int)getbitu(msg->msg, 110, 2)) return 0; /* check IODP */
blk = getbitu(msg->msg, 112, 2);
blk = getbitu(msg->msg, 112, 2);
iodf = getbitu(msg->msg, 114, 2);
for (i = 0;i<6;i++)
for (i = 0; i < 6; i++)
{
if ((j = 13*blk+i)>=sbssat->nsat) break;
udre = getbitu(msg->msg, 86+4*i, 4);
if ((j = 13 * blk + i) >= sbssat->nsat) break;
udre = getbitu(msg->msg, 86 + 4 * i, 4);
sbssat->sat[j].fcorr.t0 = gpst2time(msg->week, msg->tow);
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.t0 = gpst2time(msg->week, msg->tow);
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.iodf = iodf;
}
return decode_longcorrh(msg, 120, sbssat);
@@ -410,21 +430,21 @@ int decode_sbstype26(const sbsmsg_t *msg, sbsion_t *sbsion)
trace(4, "decode_sbstype26:\n");
if (band>MAXBAND || sbsion[band].iodi != (int)getbitu(msg->msg, 217, 2)) return 0;
if (band > MAXBAND || sbsion[band].iodi != (int)getbitu(msg->msg, 217, 2)) return 0;
block = getbitu(msg->msg, 18, 4);
for (i = 0;i<15;i++)
for (i = 0; i < 15; i++)
{
if ((j = block*15+i) >= sbsion[band].nigp) continue;
give = getbitu(msg->msg, 22+i*13+9, 4);
if ((j = block * 15 + i) >= sbsion[band].nigp) continue;
give = getbitu(msg->msg, 22 + i * 13 + 9, 4);
delay = getbitu(msg->msg, 22+i*13, 9);
delay = getbitu(msg->msg, 22 + i * 13, 9);
sbsion[band].igp[j].t0 = gpst2time(msg->week, msg->tow);
sbsion[band].igp[j].delay = delay == 0x1FF ? 0.0f : delay * 0.125f;
sbsion[band].igp[j].give = give+1;
sbsion[band].igp[j].give = give + 1;
if (sbsion[band].igp[j].give>=16)
if (sbsion[band].igp[j].give >= 16)
{
sbsion[band].igp[j].give = 0;
}
@@ -452,31 +472,52 @@ int sbsupdatecorr(const sbsmsg_t *msg, nav_t *nav)
if (msg->week == 0) return -1;
switch (type)
{
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 18: stat = decode_sbstype18(msg, nav ->sbsion); break;
case 24: stat = decode_sbstype24(msg, &nav->sbssat); break;
case 25: stat = decode_sbstype25(msg, &nav->sbssat); break;
case 26: stat = decode_sbstype26(msg, nav ->sbsion); break;
case 63: break; /* null message */
{
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 18:
stat = decode_sbstype18(msg, nav->sbsion);
break;
case 24:
stat = decode_sbstype24(msg, &nav->sbssat);
break;
case 25:
stat = decode_sbstype25(msg, &nav->sbssat);
break;
case 26:
stat = decode_sbstype26(msg, nav->sbsion);
break;
case 63:
break; /* null message */
/*default: trace(2, "unsupported sbas message: type=%d\n", type); break;*/
}
/*default: trace(2, "unsupported sbas message: type=%d\n", type); break;*/
}
return stat ? type : -1;
}
/* read sbas log file --------------------------------------------------------*/
void readmsgs(const char *file, int sel, gtime_t ts, gtime_t te,
sbs_t *sbs)
sbs_t *sbs)
{
sbsmsg_t *sbs_msgs;
int i, week, prn, ch, msg;
@@ -495,48 +536,52 @@ void readmsgs(const char *file, int sel, gtime_t ts, gtime_t te,
}
while (fgets(buff, sizeof(buff), fp))
{
if (sscanf(buff, "%d %lf %d", &week, &tow, &prn) == 3 && (p = strstr(buff, ": ")))
if (sscanf(buff, "%d %lf %d", &week, &tow, &prn) == 3 && (p = strstr(buff, ": ")))
{
p+=2; /* rtklib form */
p += 2; /* rtklib form */
}
else if (sscanf(buff, "%d %lf %lf %lf %lf %lf %lf %d",
&prn, ep, ep+1, ep+2, ep+3, ep+4, ep+5, &msg) == 8)
&prn, ep, ep + 1, ep + 2, ep + 3, ep + 4, ep + 5, &msg) == 8)
{
/* ems (EGNOS Message Service) form */
ep[0] += ep[0] < 70.0 ? 2000.0 : 1900.0;
tow = time2gpst(epoch2time(ep), &week);
p = buff + (msg >= 10 ? 25 : 24);
}
else if (!strncmp(buff,"#RAWWAASFRAMEA",14))
else if (!strncmp(buff, "#RAWWAASFRAMEA", 14))
{ /* NovAtel OEM4/V */
if (!(p = getfield(buff,6))) continue;
if (sscanf(p,"%d,%lf", &week, &tow) < 2) continue;
if (!(p = getfield(buff, 6))) continue;
if (sscanf(p, "%d,%lf", &week, &tow) < 2) continue;
if (!(p = strchr(++p, ';'))) continue;
if (sscanf(++p, "%d,%d", &ch, &prn) < 2) continue;
if (!(p = getfield(p, 4))) continue;
}
else if (!strncmp(buff,"$FRMA",5))
else if (!strncmp(buff, "$FRMA", 5))
{ /* NovAtel OEM3 */
if (!(p = getfield(buff,2))) continue;
if (!(p = getfield(buff, 2))) continue;
if (sscanf(p, "%d,%lf,%d", &week, &tow, &prn) < 3) continue;
if (!(p = getfield(p, 6))) continue;
if (week < WEEKOFFSET) week += WEEKOFFSET;
}
else continue;
else
continue;
if (sel != 0 && sel != prn) continue;
time = gpst2time(week, tow);
if (!screent(time, ts,te,0.0)) continue;
if (!screent(time, ts, te, 0.0)) continue;
if (sbs->n>=sbs->nmax)
if (sbs->n >= sbs->nmax)
{
sbs->nmax = sbs->nmax == 0 ? 1024 : sbs->nmax * 2;
if (!(sbs_msgs = (sbsmsg_t *)realloc(sbs->msgs, sbs->nmax * sizeof(sbsmsg_t))))
{
trace(1, "readsbsmsg malloc error: nmax=%d\n", sbs->nmax);
free(sbs->msgs); sbs->msgs = NULL; sbs->n = sbs->nmax = 0; fclose(fp);
free(sbs->msgs);
sbs->msgs = NULL;
sbs->n = sbs->nmax = 0;
fclose(fp);
return;
}
sbs->msgs = sbs_msgs;
@@ -545,7 +590,7 @@ void readmsgs(const char *file, int sel, gtime_t ts, gtime_t te,
sbs->msgs[sbs->n].tow = (int)(tow + 0.5);
sbs->msgs[sbs->n].prn = prn;
for (i = 0; i < 29; i++) sbs->msgs[sbs->n].msg[i] = 0;
for (i = 0; *(p-1) && *p && i < 29; p += 2, i++)
for (i = 0; *(p - 1) && *p && i < 29; p += 2, i++)
{
if (sscanf(p, "%2X", &b) == 1) sbs->msgs[sbs->n].msg[i] = (unsigned char)b;
}
@@ -558,9 +603,8 @@ void readmsgs(const char *file, int sel, gtime_t ts, gtime_t te,
/* compare sbas messages -----------------------------------------------------*/
int cmpmsgs(const void *p1, const void *p2)
{
sbsmsg_t *q1 = (sbsmsg_t *)p1,*q2 = (sbsmsg_t *)p2;
return q1->week != q2->week ? q1->week-q2->week :
(q1->tow < q2->tow ? -1 : (q1->tow > q2->tow ? 1 : q1->prn - q2->prn));
sbsmsg_t *q1 = (sbsmsg_t *)p1, *q2 = (sbsmsg_t *)p2;
return q1->week != q2->week ? q1->week - q2->week : (q1->tow < q2->tow ? -1 : (q1->tow > q2->tow ? 1 : q1->prn - q2->prn));
}
@@ -580,14 +624,14 @@ int cmpmsgs(const void *p1, const void *p2)
* .sbs, .SBS, .ems, .EMS
*-----------------------------------------------------------------------------*/
int sbsreadmsgt(const char *file, int sel, gtime_t ts, gtime_t te,
sbs_t *sbs)
sbs_t *sbs)
{
char *efiles[MAXEXFILE] = {},*ext;
int i,n;
char *efiles[MAXEXFILE] = {}, *ext;
int i, n;
trace(3,"sbsreadmsgt: file=%s sel=%d\n",file,sel);
trace(3, "sbsreadmsgt: file=%s sel=%d\n", file, sel);
for (i = 0; i < MAXEXFILE ; i++)
for (i = 0; i < MAXEXFILE; i++)
{
if (!(efiles[i] = (char *)malloc(1024)))
{
@@ -596,20 +640,20 @@ int sbsreadmsgt(const char *file, int sel, gtime_t ts, gtime_t te,
}
}
/* expand wild card in file path */
n = expath(file,efiles,MAXEXFILE);
n = expath(file, efiles, MAXEXFILE);
for (i = 0; i < n; i++)
{
if (!(ext = strrchr(efiles[i],'.'))) continue;
if (!(ext = strrchr(efiles[i], '.'))) continue;
if (strcmp(ext, ".sbs") && strcmp(ext, ".SBS") &&
strcmp(ext, ".ems") && strcmp(ext, ".EMS")) continue;
strcmp(ext, ".ems") && strcmp(ext, ".EMS")) continue;
readmsgs(efiles[i], sel, ts, te, sbs);
}
for (i = 0; i < MAXEXFILE; i++) free(efiles[i]);
/* sort messages */
if (sbs->n >0)
if (sbs->n > 0)
{
qsort(sbs->msgs, sbs->n, sizeof(sbsmsg_t), cmpmsgs);
}
@@ -637,23 +681,23 @@ void sbsoutmsg(FILE *fp, sbsmsg_t *sbsmsg)
{
int i, type = sbsmsg->msg[1] >> 2;
trace(4,"sbsoutmsg:\n");
trace(4, "sbsoutmsg:\n");
fprintf(fp, "%4d %6d %3d %2d : ", sbsmsg->week, sbsmsg->tow, sbsmsg->prn, type);
for (i = 0; i < 29; i++) fprintf(fp, "%02X", sbsmsg->msg[i]);
fprintf(fp,"\n");
fprintf(fp, "\n");
}
/* search igps ---------------------------------------------------------------*/
void searchigp(gtime_t time __attribute__((unused)), const double *pos, const sbsion_t *ion,
const sbsigp_t **igp, double *x, double *y)
const sbsigp_t **igp, double *x, double *y)
{
int i,latp[2],lonp[4];
double lat = pos[0] * R2D,lon = pos[1] * R2D;
int i, latp[2], lonp[4];
double lat = pos[0] * R2D, lon = pos[1] * R2D;
const sbsigp_t *p;
trace(4,"searchigp: pos=%.3f %.3f\n",pos[0] * R2D, pos[1] * R2D);
trace(4, "searchigp: pos=%.3f %.3f\n", pos[0] * R2D, pos[1] * R2D);
if (lon >= 180.0) lon -= 360.0;
if (-55.0 <= lat && lat < 55.0)
@@ -667,7 +711,7 @@ void searchigp(gtime_t time __attribute__((unused)), const double *pos, const sb
}
else
{
latp[0] = (int)floor((lat-5.0) / 10.0) * 10 + 5;
latp[0] = (int)floor((lat - 5.0) / 10.0) * 10 + 5;
latp[1] = latp[0] + 10;
lonp[0] = lonp[1] = (int)floor(lon / 10.0) * 10;
lonp[2] = lonp[3] = lonp[0] + 10;
@@ -692,16 +736,21 @@ void searchigp(gtime_t time __attribute__((unused)), const double *pos, const sb
for (i = 0; i < 4; i++) lonp[i] = (int)floor((lon - 50.0) / 90.0) * 90 + 40;
}
}
for (i = 0; i < 4; i++) if (lonp[i] == 180) lonp[i] = -180;
for (i = 0; i < 4; i++)
if (lonp[i] == 180) lonp[i] = -180;
for (i = 0; i <= MAXBAND; i++)
{
for (p = ion[i].igp; p < ion[i].igp + ion[i].nigp; p++)
{
if (p->t0.time == 0) continue;
if (p->lat == latp[0] && p->lon == lonp[0] && p->give > 0) igp[0] = p;
else if (p->lat == latp[1] && p->lon == lonp[1] && p->give > 0) igp[1] = p;
else if (p->lat == latp[0] && p->lon == lonp[2] && p->give > 0) igp[2] = p;
else if (p->lat == latp[1] && p->lon == lonp[3] && p->give > 0) igp[3] = p;
if (p->lat == latp[0] && p->lon == lonp[0] && p->give > 0)
igp[0] = p;
else if (p->lat == latp[1] && p->lon == lonp[1] && p->give > 0)
igp[1] = p;
else if (p->lat == latp[0] && p->lon == lonp[2] && p->give > 0)
igp[2] = p;
else if (p->lat == latp[1] && p->lon == lonp[3] && p->give > 0)
igp[3] = p;
if (igp[0] && igp[1] && igp[2] && igp[3]) return;
}
}
@@ -722,24 +771,24 @@ void searchigp(gtime_t time __attribute__((unused)), const double *pos, const sb
* sbsupdatecorr()
*-----------------------------------------------------------------------------*/
int sbsioncorr(gtime_t time, const nav_t *nav, const double *pos,
const double *azel, double *delay, double *var)
const double *azel, double *delay, double *var)
{
const double re = 6378.1363,hion = 350.0;
int i,err = 0;
double fp,posp[2],x = 0.0,y = 0.0,t,w[4] = {};
const double re = 6378.1363, hion = 350.0;
int i, err = 0;
double fp, posp[2], x = 0.0, y = 0.0, t, w[4] = {};
const sbsigp_t *igp[4] = {}; /* {ws,wn,es,en} */
trace(4,"sbsioncorr: pos=%.3f %.3f azel=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D,
azel[0]*R2D,azel[1]*R2D);
trace(4, "sbsioncorr: pos=%.3f %.3f azel=%.3f %.3f\n", pos[0] * R2D, pos[1] * R2D,
azel[0] * R2D, azel[1] * R2D);
*delay = *var = 0.0;
if (pos[2]<-100.0||azel[1]<=0) return 1;
if (pos[2] < -100.0 || azel[1] <= 0) return 1;
/* ipp (ionospheric pierce point) position */
fp = ionppp(pos,azel,re,hion,posp);
fp = ionppp(pos, azel, re, hion, posp);
/* search igps around ipp */
searchigp(time,posp,nav->sbsion,igp,&x,&y);
searchigp(time, posp, nav->sbsion, igp, &x, &y);
/* weight of igps */
if (igp[0] && igp[1] && igp[2] && igp[3])
@@ -751,42 +800,48 @@ int sbsioncorr(gtime_t time, const nav_t *nav, const double *pos,
}
else if (igp[0] && igp[1] && igp[2])
{
w[1] = y; w[2] = x;
if ((w[0] = 1.0- w[1] - w[2]) < 0.0) err = 1;
w[1] = y;
w[2] = x;
if ((w[0] = 1.0 - w[1] - w[2]) < 0.0) err = 1;
}
else if (igp[0] && igp[2] && igp[3])
{
w[0] = 1.0 - x; w[3] = y;
w[0] = 1.0 - x;
w[3] = y;
if ((w[2] = 1.0 - w[0] - w[3]) < 0.0) err = 1;
}
else if (igp[0] && igp[1] && igp[3])
{
w[0] = 1.0 - y; w[3] = x;
w[0] = 1.0 - y;
w[3] = x;
if ((w[1] = 1.0 - w[0] - w[3]) < 0.0) err = 1;
}
else if (igp[1] && igp[2] && igp[3])
{
w[1] = 1.0-x; w[2] = 1.0-y;
w[1] = 1.0 - x;
w[2] = 1.0 - y;
if ((w[3] = 1.0 - w[1] - w[2]) < 0.0) err = 1;
}
else err = 1;
else
err = 1;
if (err)
{
trace(2, "no sbas iono correction: lat=%3.0f lon=%4.0f\n", posp[0] * R2D,
posp[1] * R2D);
posp[1] * R2D);
return 0;
}
for (i = 0; i < 4; i++)
{
if (!igp[i]) continue;
t = timediff(time,igp[i]->t0);
t = timediff(time, igp[i]->t0);
*delay += w[i] * igp[i]->delay;
*var += w[i] * varicorr(igp[i]->give) * 9e-8 * fabs(t);
}
*delay *= fp; *var *= fp * fp;
*delay *= fp;
*var *= fp * fp;
trace(5,"sbsioncorr: dion=%7.2f sig=%7.2f\n",*delay,sqrt(*var));
trace(5, "sbsioncorr: dion=%7.2f sig=%7.2f\n", *delay, sqrt(*var));
return 1;
}
@@ -794,22 +849,24 @@ int sbsioncorr(gtime_t time, const nav_t *nav, const double *pos,
/* get meterological parameters ----------------------------------------------*/
void getmet(double lat, double *met)
{
static const double metprm[][10] = { /* lat=15,30,45,60,75 */
{1013.25,299.65,26.31,6.30E-3,2.77, 0.00, 0.00,0.00,0.00E-3,0.00},
{1017.25,294.15,21.79,6.05E-3,3.15, -3.75, 7.00,8.85,0.25E-3,0.33},
{1015.75,283.15,11.66,5.58E-3,2.57, -2.25,11.00,7.24,0.32E-3,0.46},
{1011.75,272.15, 6.78,5.39E-3,1.81, -1.75,15.00,5.36,0.81E-3,0.74},
{1013.00,263.65, 4.11,4.53E-3,1.55, -0.50,14.50,3.39,0.62E-3,0.30}
};
int i,j;
static const double metprm[][10] = {/* lat=15,30,45,60,75 */
{1013.25, 299.65, 26.31, 6.30E-3, 2.77, 0.00, 0.00, 0.00, 0.00E-3, 0.00},
{1017.25, 294.15, 21.79, 6.05E-3, 3.15, -3.75, 7.00, 8.85, 0.25E-3, 0.33},
{1015.75, 283.15, 11.66, 5.58E-3, 2.57, -2.25, 11.00, 7.24, 0.32E-3, 0.46},
{1011.75, 272.15, 6.78, 5.39E-3, 1.81, -1.75, 15.00, 5.36, 0.81E-3, 0.74},
{1013.00, 263.65, 4.11, 4.53E-3, 1.55, -0.50, 14.50, 3.39, 0.62E-3, 0.30}};
int i, j;
double a;
lat = fabs(lat);
if (lat <= 15.0) for (i = 0;i<10;i++) met[i] = metprm[0][i];
else if (lat >= 75.0) for (i = 0;i<10;i++) met[i] = metprm[4][i];
if (lat <= 15.0)
for (i = 0; i < 10; i++) met[i] = metprm[0][i];
else if (lat >= 75.0)
for (i = 0; i < 10; i++) met[i] = metprm[4][i];
else
{
j = (int)(lat / 15.0); a = (lat - j * 15.0) / 15.0;
for (i = 0; i < 10; i++) met[i] = (1.0 - a) * metprm[j-1][i] + a * metprm[j][i];
j = (int)(lat / 15.0);
a = (lat - j * 15.0) / 15.0;
for (i = 0; i < 10; i++) met[i] = (1.0 - a) * metprm[j - 1][i] + a * metprm[j][i];
}
}
@@ -823,15 +880,15 @@ void getmet(double lat, double *met)
* return : slant tropospheric delay (m)
*-----------------------------------------------------------------------------*/
double sbstropcorr(gtime_t time, const double *pos, const double *azel,
double *var)
double *var)
{
const double k1 = 77.604,k2 = 382000.0,rd = 287.054,gm = 9.784,g = 9.80665;
const double k1 = 77.604, k2 = 382000.0, rd = 287.054, gm = 9.784, g = 9.80665;
static double pos_[3] = {}, zh = 0.0, zw = 0.0;
int i;
double c, met[10], sinel = sin(azel[1]), h = pos[2], m;
trace(4, "sbstropcorr: pos=%.3f %.3f azel=%.3f %.3f\n", pos[0] * R2D, pos[1] * R2D,
azel[0] * R2D, azel[1] * R2D);
azel[0] * R2D, azel[1] * R2D);
if (pos[2] < -100.0 || 10000.0 < pos[2] || azel[1] <= 0)
{
@@ -839,71 +896,71 @@ double sbstropcorr(gtime_t time, const double *pos, const double *azel,
return 0.0;
}
if (zh == 0.0 || fabs(pos[0] - pos_[0]) > 1e-7 || fabs(pos[1] - pos_[1]) > 1e-7 ||
fabs(pos[2] - pos_[2]) > 1.0)
fabs(pos[2] - pos_[2]) > 1.0)
{
getmet(pos[0] * R2D,met);
getmet(pos[0] * R2D, met);
c = cos(2.0 * PI * (time2doy(time) - (pos[0] >= 0.0 ? 28.0 : 211.0)) / 365.25);
for (i = 0; i < 5;i++) met[i] -= met[i+5] * c;
for (i = 0; i < 5; i++) met[i] -= met[i + 5] * c;
zh = 1e-6 * k1 * rd * met[0] / gm;
zw = 1e-6 * k2 * rd / (gm * (met[4] + 1.0) - met[3] * rd) * met[2] / met[1];
zh *= pow(1.0 - met[3] * h / met[1], g / (rd * met[3]));
zw *= pow(1.0 - met[3] * h / met[1], (met[4] + 1.0) * g / (rd * met[3]) - 1.0);
for (i = 0; i < 3; i++) pos_[i] = pos[i];
}
m = 1.001/sqrt(0.002001+sinel*sinel);
*var = 0.12*0.12*m*m;
return (zh+zw)*m;
m = 1.001 / sqrt(0.002001 + sinel * sinel);
*var = 0.12 * 0.12 * m * m;
return (zh + zw) * m;
}
/* long term correction ------------------------------------------------------*/
int sbslongcorr(gtime_t time, int sat, const sbssat_t *sbssat,
double *drs, double *ddts)
double *drs, double *ddts)
{
const sbssatp_t *p;
double t;
int i;
trace(3,"sbslongcorr: sat=%2d\n",sat);
trace(3, "sbslongcorr: sat=%2d\n", sat);
for (p = sbssat->sat;p<sbssat->sat+sbssat->nsat;p++)
for (p = sbssat->sat; p < sbssat->sat + sbssat->nsat; p++)
{
if (p->sat != sat || p->lcorr.t0.time == 0) continue;
t = timediff(time, p->lcorr.t0);
if (fabs(t) > MAXSBSAGEL)
{
trace(2,"sbas long-term correction expired: %s sat=%2d t=%5.0f\n",
time_str(time,0), sat, t);
trace(2, "sbas long-term correction expired: %s sat=%2d t=%5.0f\n",
time_str(time, 0), sat, t);
return 0;
}
for (i = 0; i < 3;i++) drs[i] = p->lcorr.dpos[i] + p->lcorr.dvel[i] * t;
for (i = 0; i < 3; i++) drs[i] = p->lcorr.dpos[i] + p->lcorr.dvel[i] * t;
*ddts = p->lcorr.daf0 + p->lcorr.daf1 * t;
trace(5,"sbslongcorr: sat=%2d drs=%7.2f%7.2f%7.2f ddts=%7.2f\n",
sat, drs[0], drs[1], drs[2], *ddts * SPEED_OF_LIGHT);
trace(5, "sbslongcorr: sat=%2d drs=%7.2f%7.2f%7.2f ddts=%7.2f\n",
sat, drs[0], drs[1], drs[2], *ddts * SPEED_OF_LIGHT);
return 1;
}
/* if sbas satellite without correction, no correction applied */
if (satsys(sat,NULL) == SYS_SBS) return 1;
if (satsys(sat, NULL) == SYS_SBS) return 1;
trace(2,"no sbas long-term correction: %s sat=%2d\n",time_str(time,0),sat);
trace(2, "no sbas long-term correction: %s sat=%2d\n", time_str(time, 0), sat);
return 0;
}
/* fast correction -----------------------------------------------------------*/
int sbsfastcorr(gtime_t time, int sat, const sbssat_t *sbssat,
double *prc, double *var)
double *prc, double *var)
{
const sbssatp_t *p;
double t;
trace(3,"sbsfastcorr: sat=%2d\n",sat);
trace(3, "sbsfastcorr: sat=%2d\n", sat);
for (p = sbssat->sat;p<sbssat->sat+sbssat->nsat;p++)
for (p = sbssat->sat; p < sbssat->sat + sbssat->nsat; p++)
{
if (p->sat!=sat) continue;
if (p->sat != sat) continue;
if (p->fcorr.t0.time == 0) break;
t = timediff(time, p->fcorr.t0) + sbssat->tlat;
@@ -918,11 +975,11 @@ int sbsfastcorr(gtime_t time, int sat, const sbssat_t *sbssat,
#endif
*var = varfcorr(p->fcorr.udre) + degfcorr(p->fcorr.ai) * t * t / 2.0;
trace(5,"sbsfastcorr: sat=%3d prc=%7.2f sig=%7.2f t=%5.0f\n",sat,
*prc, sqrt(*var), t);
trace(5, "sbsfastcorr: sat=%3d prc=%7.2f sig=%7.2f t=%5.0f\n", sat,
*prc, sqrt(*var), t);
return 1;
}
trace(2,"no sbas fast correction: %s sat=%2d\n", time_str(time, 0), sat);
trace(2, "no sbas fast correction: %s sat=%2d\n", time_str(time, 0), sat);
return 0;
}
@@ -945,12 +1002,12 @@ int sbsfastcorr(gtime_t time, int sat, const sbssat_t *sbssat,
* to be considered for other codes
*-----------------------------------------------------------------------------*/
int sbssatcorr(gtime_t time, int sat, const nav_t *nav, double *rs,
double *dts, double *var)
double *dts, double *var)
{
double drs[3] = {}, dclk = 0.0, prc = 0.0;
int i;
trace(3,"sbssatcorr : sat=%2d\n",sat);
trace(3, "sbssatcorr : sat=%2d\n", sat);
/* sbas long term corrections */
if (!sbslongcorr(time, sat, &nav->sbssat, drs, &dclk))
@@ -966,8 +1023,8 @@ int sbssatcorr(gtime_t time, int sat, const nav_t *nav, double *rs,
dts[0] += dclk + prc / SPEED_OF_LIGHT;
trace(5,"sbssatcorr: sat=%2d drs=%6.3f %6.3f %6.3f dclk=%.3f %.3f var=%.3f\n",
sat, drs[0], drs[1], drs[2], dclk, prc / SPEED_OF_LIGHT, *var);
trace(5, "sbssatcorr: sat=%2d drs=%6.3f %6.3f %6.3f dclk=%.3f %.3f var=%.3f\n",
sat, drs[0], drs[1], drs[2], dclk, prc / SPEED_OF_LIGHT, *var);
return 1;
}
@@ -982,24 +1039,25 @@ int sbssatcorr(gtime_t time, int sat, const nav_t *nav, double *rs,
* return : status (1:ok,0:crc error)
*-----------------------------------------------------------------------------*/
int sbsdecodemsg(gtime_t time, int prn, const unsigned int *words,
sbsmsg_t *sbsmsg)
sbsmsg_t *sbsmsg)
{
int i,j;
int i, j;
unsigned char f[29];
double tow;
trace(5,"sbsdecodemsg: prn=%d\n",prn);
trace(5, "sbsdecodemsg: prn=%d\n", prn);
if (time.time == 0) return 0;
tow = time2gpst(time,&sbsmsg->week);
tow = time2gpst(time, &sbsmsg->week);
sbsmsg->tow = (int)(tow + DTTOL);
sbsmsg->prn = prn;
for (i = 0; i < 7; i++) for (j = 0; j < 4; j++)
{
sbsmsg->msg[i*4+j] = (unsigned char)(words[i] >> ((3-j)*8));
}
sbsmsg->msg[28] = (unsigned char)(words[7] >> 18 ) & 0xC0;
for (i = 28; i > 0; i--) f[i] = (sbsmsg->msg[i] >> 6) + (sbsmsg->msg[i-1] << 2);
for (i = 0; i < 7; i++)
for (j = 0; j < 4; j++)
{
sbsmsg->msg[i * 4 + j] = (unsigned char)(words[i] >> ((3 - j) * 8));
}
sbsmsg->msg[28] = (unsigned char)(words[7] >> 18) & 0xC0;
for (i = 28; i > 0; i--) f[i] = (sbsmsg->msg[i] >> 6) + (sbsmsg->msg[i - 1] << 2);
f[0] = sbsmsg->msg[0] >> 6;
return rtk_crc24q(f, 29) == (words[7] & 0xFFFFFF); /* check crc */

108
src/algorithms/libs/rtklib/rtklib_sbas.h
View File

@@ -66,56 +66,54 @@
/* constants -----------------------------------------------------------------*/
const int WEEKOFFSET = 1024; /* gps week offset for NovAtel OEM-3 */
const int WEEKOFFSET = 1024; /* gps week offset for NovAtel OEM-3 */
/* sbas igp definition -------------------------------------------------------*/
static const short
x1[] = {-75, -65, -55, -50, -45, -40, -35, -30, -25, -20, -15, -10, - 5, 0, 5, 10, 15, 20,
25, 30, 35, 40, 45, 50, 55, 65, 75, 85},
x2[] = {-55, -50, -45, -40, -35, -30, -25, -20, -15, -10, -5, 0, 5, 10, 15, 20, 25, 30,
35, 40, 45, 50, 55},
x3[] = {-75, -65, -55, -50, -45, -40, -35, -30, -25, -20, -15, -10, - 5, 0, 5, 10, 15, 20,
25, 30, 35, 40, 45, 50, 55, 65, 75},
x4[] = {-85, -75, -65, -55, -50, -45, -40, -35, -30, -25, -20, -15, -10, - 5, 0, 5, 10, 15,
20, 25, 30, 35, 40, 45, 50, 55, 65, 75},
x5[] = {-180, -175, -170, -165, -160, -155, -150, -145, -140, -135, -130, -125, -120, -115,
-110, -105, -100, - 95, - 90, - 85, - 80, - 75, - 70, - 65, - 60, - 55, - 50, - 45,
- 40, - 35, - 30, - 25, - 20, - 15, - 10, - 5, 0, 5, 10, 15, 20, 25,
30, 35, 40, 45, 50, 55, 60, 65, 70, 75, 80, 85, 90, 95,
100, 105, 110, 115, 120, 125, 130, 135, 140, 145, 150, 155, 160, 165,
170, 175},
x6[] = {-180, -170, -160, -150, -140, -130, -120, -110, -100, - 90, - 80, - 70, - 60, - 50,
- 40, - 30, - 20, - 10, 0, 10, 20, 30, 40, 50, 60, 70, 80, 90,
100, 110, 120, 130, 140, 150, 160, 170},
x7[] = {-180, -150, -120, - 90, - 60, - 30, 0, 30, 60, 90, 120, 150},
x8[] = {-170, -140, -110, - 80, - 50, - 20, 10, 40, 70, 100, 130, 160};
x1[] = {-75, -65, -55, -50, -45, -40, -35, -30, -25, -20, -15, -10, -5, 0, 5, 10, 15, 20,
25, 30, 35, 40, 45, 50, 55, 65, 75, 85},
x2[] = {-55, -50, -45, -40, -35, -30, -25, -20, -15, -10, -5, 0, 5, 10, 15, 20, 25, 30,
35, 40, 45, 50, 55},
x3[] = {-75, -65, -55, -50, -45, -40, -35, -30, -25, -20, -15, -10, -5, 0, 5, 10, 15, 20,
25, 30, 35, 40, 45, 50, 55, 65, 75},
x4[] = {-85, -75, -65, -55, -50, -45, -40, -35, -30, -25, -20, -15, -10, -5, 0, 5, 10, 15,
20, 25, 30, 35, 40, 45, 50, 55, 65, 75},
x5[] = {-180, -175, -170, -165, -160, -155, -150, -145, -140, -135, -130, -125, -120, -115,
-110, -105, -100, -95, -90, -85, -80, -75, -70, -65, -60, -55, -50, -45,
-40, -35, -30, -25, -20, -15, -10, -5, 0, 5, 10, 15, 20, 25,
30, 35, 40, 45, 50, 55, 60, 65, 70, 75, 80, 85, 90, 95,
100, 105, 110, 115, 120, 125, 130, 135, 140, 145, 150, 155, 160, 165,
170, 175},
x6[] = {-180, -170, -160, -150, -140, -130, -120, -110, -100, -90, -80, -70, -60, -50,
-40, -30, -20, -10, 0, 10, 20, 30, 40, 50, 60, 70, 80, 90,
100, 110, 120, 130, 140, 150, 160, 170},
x7[] = {-180, -150, -120, -90, -60, -30, 0, 30, 60, 90, 120, 150},
x8[] = {-170, -140, -110, -80, -50, -20, 10, 40, 70, 100, 130, 160};
const sbsigpband_t igpband1[9][8] = { /* band 0-8 */
{{-180, x1, 1, 28}, {-175, x2, 29, 51}, {-170, x3, 52, 78}, {-165, x2, 79, 101},
{-160, x3, 102, 128}, {-155, x2, 129, 151}, {-150, x3, 152, 178}, {-145, x2, 179, 201}},
{{-140, x4, 1, 28}, {-135, x2, 29, 51}, {-130, x3, 52, 78}, {-125, x2, 79, 101},
{-120, x3, 102, 128}, {-115, x2, 129, 151}, {-110, x3, 152, 178}, {-105, x2, 179, 201}},
{{-100, x3, 1, 27}, {- 95,x2, 28, 50},{- 90, x1, 51, 78}, {- 85, x2, 79, 101},
{- 80, x3, 102, 128}, {- 75, x2, 129, 151}, {- 70, x3, 152, 178}, {- 65, x2,179, 201}},
{{- 60, x3, 1, 27}, {- 55, x2, 28, 50}, {- 50,x4, 51, 78}, {- 45, x2, 79, 101},
{- 40, x3, 102, 128}, {- 35, x2, 129, 151}, {- 30, x3, 152, 178}, {- 25, x2, 179, 201}},
{{- 20, x3, 1, 27}, {- 15, x2, 28, 50}, {- 10, x3, 51, 77}, {- 5, x2, 78, 100},
{ 0, x1, 101, 128}, { 5, x2, 129, 151}, { 10, x3, 152, 178}, { 15, x2, 179, 201}},
{{ 20, x3, 1, 27}, { 25, x2, 28, 50}, { 30, x3, 51, 77}, { 35, x2, 78, 100},
{ 40, x4, 101, 128}, { 45, x2, 129, 151},{ 50, x3, 152, 178}, { 55, x2, 179, 201}},
{{ 60, x3, 1, 27}, { 65, x2, 28, 50}, { 70, x3, 51, 77}, { 75, x2, 78, 100},
{ 80, x3, 101, 127}, { 85, x2, 128, 150}, { 90,x1, 151, 178}, { 95, x2, 179, 201}},
{{ 100, x3, 1, 27}, { 105, x2, 28, 50}, { 110, x3, 51, 77}, { 115, x2, 78 ,100},
{ 120, x3, 101, 127}, { 125, x2, 128, 150}, { 130, x4, 151, 178}, { 135, x2, 179, 201}},
{{ 140, x3, 1, 27}, { 145, x2, 28, 50}, { 150, x3, 51, 77}, { 155, x2, 78, 100},
{ 160, x3, 101, 127}, { 165, x2,128, 150}, { 170, x3,151,177}, { 175, x2, 178, 200}}
};
const sbsigpband_t igpband2[2][5] = { /* band 9-10 */
{{ 60, x5, 1, 72}, { 65, x6, 73, 108}, { 70, x6, 109, 144}, { 75, x6, 145, 180},
{ 85, x7, 181, 192}},
{{- 60, x5, 1, 72}, {- 65, x6, 73, 108}, {- 70, x6, 109, 144}, {- 75 ,x6, 145, 180},
{- 85, x8, 181, 192}}
};
const sbsigpband_t igpband1[9][8] = {/* band 0-8 */
{{-180, x1, 1, 28}, {-175, x2, 29, 51}, {-170, x3, 52, 78}, {-165, x2, 79, 101},
{-160, x3, 102, 128}, {-155, x2, 129, 151}, {-150, x3, 152, 178}, {-145, x2, 179, 201}},
{{-140, x4, 1, 28}, {-135, x2, 29, 51}, {-130, x3, 52, 78}, {-125, x2, 79, 101},
{-120, x3, 102, 128}, {-115, x2, 129, 151}, {-110, x3, 152, 178}, {-105, x2, 179, 201}},
{{-100, x3, 1, 27}, {-95, x2, 28, 50}, {-90, x1, 51, 78}, {-85, x2, 79, 101},
{-80, x3, 102, 128}, {-75, x2, 129, 151}, {-70, x3, 152, 178}, {-65, x2, 179, 201}},
{{-60, x3, 1, 27}, {-55, x2, 28, 50}, {-50, x4, 51, 78}, {-45, x2, 79, 101},
{-40, x3, 102, 128}, {-35, x2, 129, 151}, {-30, x3, 152, 178}, {-25, x2, 179, 201}},
{{-20, x3, 1, 27}, {-15, x2, 28, 50}, {-10, x3, 51, 77}, {-5, x2, 78, 100},
{0, x1, 101, 128}, {5, x2, 129, 151}, {10, x3, 152, 178}, {15, x2, 179, 201}},
{{20, x3, 1, 27}, {25, x2, 28, 50}, {30, x3, 51, 77}, {35, x2, 78, 100},
{40, x4, 101, 128}, {45, x2, 129, 151}, {50, x3, 152, 178}, {55, x2, 179, 201}},
{{60, x3, 1, 27}, {65, x2, 28, 50}, {70, x3, 51, 77}, {75, x2, 78, 100},
{80, x3, 101, 127}, {85, x2, 128, 150}, {90, x1, 151, 178}, {95, x2, 179, 201}},
{{100, x3, 1, 27}, {105, x2, 28, 50}, {110, x3, 51, 77}, {115, x2, 78, 100},
{120, x3, 101, 127}, {125, x2, 128, 150}, {130, x4, 151, 178}, {135, x2, 179, 201}},
{{140, x3, 1, 27}, {145, x2, 28, 50}, {150, x3, 51, 77}, {155, x2, 78, 100},
{160, x3, 101, 127}, {165, x2, 128, 150}, {170, x3, 151, 177}, {175, x2, 178, 200}}};
const sbsigpband_t igpband2[2][5] = {/* band 9-10 */
{{60, x5, 1, 72}, {65, x6, 73, 108}, {70, x6, 109, 144}, {75, x6, 145, 180},
{85, x7, 181, 192}},
{{-60, x5, 1, 72}, {-65, x6, 73, 108}, {-70, x6, 109, 144}, {-75, x6, 145, 180},
{-85, x8, 181, 192}}};
char *getfield(char *p, int pos);
@@ -137,29 +135,29 @@ int decode_sbstype25(const sbsmsg_t *msg, sbssat_t *sbssat);
int decode_sbstype26(const sbsmsg_t *msg, sbsion_t *sbsion);
int sbsupdatecorr(const sbsmsg_t *msg, nav_t *nav);
void readmsgs(const char *file, int sel, gtime_t ts, gtime_t te,sbs_t *sbs);
void readmsgs(const char *file, int sel, gtime_t ts, gtime_t te, sbs_t *sbs);
int cmpmsgs(const void *p1, const void *p2);
int sbsreadmsgt(const char *file, int sel, gtime_t ts, gtime_t te,
sbs_t *sbs);
sbs_t *sbs);
int sbsreadmsg(const char *file, int sel, sbs_t *sbs);
void sbsoutmsg(FILE *fp, sbsmsg_t *sbsmsg);
void searchigp(gtime_t time, const double *pos, const sbsion_t *ion,
const sbsigp_t **igp, double *x, double *y);
const sbsigp_t **igp, double *x, double *y);
int sbsioncorr(gtime_t time, const nav_t *nav, const double *pos,
const double *azel, double *delay, double *var);
const double *azel, double *delay, double *var);
void getmet(double lat, double *met);
double sbstropcorr(gtime_t time, const double *pos, const double *azel,
double *var);
double *var);
int sbslongcorr(gtime_t time, int sat, const sbssat_t *sbssat,
double *drs, double *ddts);
double *drs, double *ddts);
int sbsfastcorr(gtime_t time, int sat, const sbssat_t *sbssat,
double *prc, double *var);
double *prc, double *var);
int sbssatcorr(gtime_t time, int sat, const nav_t *nav, double *rs,
double *dts, double *var);
double *dts, double *var);
int sbsdecodemsg(gtime_t time, int prn, const unsigned int *words,
sbsmsg_t *sbsmsg);
sbsmsg_t *sbsmsg);
#endif /* GNSS_SDR_RTKLIB_SBAS_H_ */

1405
src/algorithms/libs/rtklib/rtklib_solution.cc
View File

File diff suppressed because it is too large Load Diff

31
src/algorithms/libs/rtklib/rtklib_solution.h
View File

@@ -55,8 +55,8 @@
#include "rtklib.h"
#define COMMENTH "%" /* comment line indicator for solution */
#define MSG_DISCONN "$_DISCONNECT\r\n" /* disconnect message */
#define COMMENTH "%" /* comment line indicator for solution */
#define MSG_DISCONN "$_DISCONNECT\r\n" /* disconnect message */
const char *opt2sep(const solopt_t *opt);
@@ -99,17 +99,17 @@ void decode_solopt(char *buff, solopt_t *opt);
void readsolopt(FILE *fp, solopt_t *opt);
int inputsol(unsigned char data, gtime_t ts, gtime_t te, double tint,
int qflag, const solopt_t *opt, solbuf_t *solbuf);
int qflag, const solopt_t *opt, solbuf_t *solbuf);
int readsoldata(FILE *fp, gtime_t ts, gtime_t te, double tint, int qflag,
const solopt_t *opt, solbuf_t *solbuf);
const solopt_t *opt, solbuf_t *solbuf);
int cmpsol(const void *p1, const void *p2);
int sort_solbuf(solbuf_t *solbuf);
int readsolt(char *files[], int nfile, gtime_t ts, gtime_t te,
double tint, int qflag, solbuf_t *solbuf);
double tint, int qflag, solbuf_t *solbuf);
int readsol(char *files[], int nfile, solbuf_t *sol);
@@ -132,51 +132,50 @@ int decode_solstat(char *buff, solstat_t *stat);
void addsolstat(solstatbuf_t *statbuf, const solstat_t *stat);
int readsolstatdata(FILE *fp, gtime_t ts, gtime_t te, double tint,
solstatbuf_t *statbuf);
solstatbuf_t *statbuf);
int readsolstatt(char *files[], int nfile, gtime_t ts, gtime_t te,
double tint, solstatbuf_t *statbuf);
double tint, solstatbuf_t *statbuf);
int readsolstat(char *files[], int nfile, solstatbuf_t *statbuf);
int outecef(unsigned char *buff, const char *s, const sol_t *sol,
const solopt_t *opt);
const solopt_t *opt);
int outpos(unsigned char *buff, const char *s, const sol_t *sol, const solopt_t *opt);
int outenu(unsigned char *buff, const char *s, const sol_t *sol,
const double *rb, const solopt_t *opt);
const double *rb, const solopt_t *opt);
int outnmea_rmc(unsigned char *buff, const sol_t *sol);
int outnmea_gga(unsigned char *buff, const sol_t *sol);
int outnmea_gsa(unsigned char *buff, const sol_t *sol,
const ssat_t *ssat);
const ssat_t *ssat);
int outnmea_gsv(unsigned char *buff, const sol_t *sol,
const ssat_t *ssat);
const ssat_t *ssat);
int outprcopts(unsigned char *buff, const prcopt_t *opt);
int outsolheads(unsigned char *buff, const solopt_t *opt);
int outsols(unsigned char *buff, const sol_t *sol, const double *rb,
const solopt_t *opt);
const solopt_t *opt);
int outsolexs(unsigned char *buff, const sol_t *sol, const ssat_t *ssat,
const solopt_t *opt);
const solopt_t *opt);
void outprcopt(FILE *fp, const prcopt_t *opt);
void outsolhead(FILE *fp, const solopt_t *opt);
void outsol(FILE *fp, const sol_t *sol, const double *rb,
const solopt_t *opt);
const solopt_t *opt);
void outsolex(FILE *fp, const sol_t *sol, const ssat_t *ssat,
const solopt_t *opt);
const solopt_t *opt);
#endif

677
src/algorithms/libs/rtklib/rtklib_stream.cc
View File

File diff suppressed because it is too large Load Diff

46
src/algorithms/libs/rtklib/rtklib_stream.h
View File

@@ -50,34 +50,34 @@
*
*-----------------------------------------------------------------------------*/
#ifndef GNSS_SDR_RTKLIB_STREAM_H_
#define GNSS_SDR_RTKLIB_STREAM_H_
#ifndef GNSS_SDR_RTKLIB_STREAM_H_
#define GNSS_SDR_RTKLIB_STREAM_H_
#include "rtklib.h"
/* constants -----------------------------------------------------------------*/
#define TINTACT 200 /* period for stream active (ms) */
#define SERIBUFFSIZE 4096 /* serial buffer size (bytes) */
#define TIMETAGH_LEN 64 /* time tag file header length */
#define MAXCLI 32 /* max client connection for tcp svr */
#define MAXSTATMSG 32 /* max length of status message */
#define TINTACT 200 /* period for stream active (ms) */
#define SERIBUFFSIZE 4096 /* serial buffer size (bytes) */
#define TIMETAGH_LEN 64 /* time tag file header length */
#define MAXCLI 32 /* max client connection for tcp svr */
#define MAXSTATMSG 32 /* max length of status message */
#define VER_RTKLIB "2.4.2"
#define NTRIP_AGENT "RTKLIB/" VER_RTKLIB
#define NTRIP_CLI_PORT 2101 /* default ntrip-client connection port */
#define NTRIP_SVR_PORT 80 /* default ntrip-server connection port */
#define NTRIP_MAXRSP 32768 /* max size of ntrip response */
#define NTRIP_MAXSTR 256 /* max length of mountpoint string */
#define NTRIP_RSP_OK_CLI "ICY 200 OK\r\n" /* ntrip response: client */
#define NTRIP_RSP_OK_SVR "OK\r\n" /* ntrip response: server */
#define NTRIP_RSP_SRCTBL "SOURCETABLE 200 OK\r\n" /* ntrip response: source table */
#define NTRIP_RSP_TBLEND "ENDSOURCETABLE"
#define NTRIP_RSP_HTTP "HTTP/" /* ntrip response: http */
#define NTRIP_RSP_ERROR "ERROR" /* ntrip response: error */
#define VER_RTKLIB "2.4.2"
#define NTRIP_AGENT "RTKLIB/" VER_RTKLIB
#define NTRIP_CLI_PORT 2101 /* default ntrip-client connection port */
#define NTRIP_SVR_PORT 80 /* default ntrip-server connection port */
#define NTRIP_MAXRSP 32768 /* max size of ntrip response */
#define NTRIP_MAXSTR 256 /* max length of mountpoint string */
#define NTRIP_RSP_OK_CLI "ICY 200 OK\r\n" /* ntrip response: client */
#define NTRIP_RSP_OK_SVR "OK\r\n" /* ntrip response: server */
#define NTRIP_RSP_SRCTBL "SOURCETABLE 200 OK\r\n" /* ntrip response: source table */
#define NTRIP_RSP_TBLEND "ENDSOURCETABLE"
#define NTRIP_RSP_HTTP "HTTP/" /* ntrip response: http */
#define NTRIP_RSP_ERROR "ERROR" /* ntrip response: error */
#define FTP_CMD "wget" /* ftp/http command */
#define FTP_TIMEOUT 30 /* ftp/http timeout (s) */
#define FTP_CMD "wget" /* ftp/http command */
#define FTP_TIMEOUT 30 /* ftp/http timeout (s) */
serial_t *openserial(const char *path, int mode, char *msg);
@@ -111,7 +111,7 @@ int writefile(file_t *file, unsigned char *buff, int n, char *msg);
void syncfile(file_t *file1, file_t *file2);
void decodetcppath(const char *path, char *addr, char *port, char *user,
char *passwd, char *mntpnt, char *str);
char *passwd, char *mntpnt, char *str);
int errsock(void);
@@ -182,7 +182,7 @@ int writentrip(ntrip_t *ntrip, unsigned char *buff, int n, char *msg);
int statentrip(ntrip_t *ntrip);
void decodeftppath(const char *path, char *addr, char *file, char *user,
char *passwd, int *topts);
char *passwd, int *topts);
gtime_t nextdltime(const int *topts, int stat);

270
src/algorithms/libs/rtklib/rtklib_tides.cc
View File

@@ -54,84 +54,88 @@
#include "rtklib_rtkcmn.h"
/* solar/lunar tides (ref [2] 7) ---------------------------------------------*/
//#ifndef IERS_MODEL
void tide_pl(const double *eu, const double *rp, double GMp,
const double *pos, double *dr)
const double *pos, double *dr)
{
const double H3=0.292,L3=0.015;
double r,ep[3],latp,lonp,p,K2,K3,a,H2,L2,dp,du,cosp,sinl,cosl;
const double H3 = 0.292, L3 = 0.015;
double r, ep[3], latp, lonp, p, K2, K3, a, H2, L2, dp, du, cosp, sinl, cosl;
int i;
trace(4,"tide_pl : pos=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D);
trace(4, "tide_pl : pos=%.3f %.3f\n", pos[0] * R2D, pos[1] * R2D);
if ((r=norm_rtk(rp,3))<=0.0) return;
if ((r = norm_rtk(rp, 3)) <= 0.0) return;
for (i=0;i<3;i++) ep[i]=rp[i]/r;
for (i = 0; i < 3; i++) ep[i] = rp[i] / r;
K2=GMp/GME*std::pow(RE_WGS84, 2.04)*std::pow(RE_WGS84, 2.0)/(r*r*r);
K3=K2*RE_WGS84/r;
latp=asin(ep[2]); lonp=atan2(ep[1],ep[0]);
cosp=cos(latp); sinl=sin(pos[0]); cosl=cos(pos[0]);
K2 = GMp / GME * std::pow(RE_WGS84, 2.04) * std::pow(RE_WGS84, 2.0) / (r * r * r);
K3 = K2 * RE_WGS84 / r;
latp = asin(ep[2]);
lonp = atan2(ep[1], ep[0]);
cosp = cos(latp);
sinl = sin(pos[0]);
cosl = cos(pos[0]);
/* step1 in phase (degree 2) */
p=(3.0*sinl*sinl-1.0)/2.0;
H2=0.6078-0.0006*p;
L2=0.0847+0.0002*p;
a=dot(ep,eu,3);
dp=K2*3.0*L2*a;
du=K2*(H2*(1.5*a*a-0.5)-3.0*L2*a*a);
p = (3.0 * sinl * sinl - 1.0) / 2.0;
H2 = 0.6078 - 0.0006 * p;
L2 = 0.0847 + 0.0002 * p;
a = dot(ep, eu, 3);
dp = K2 * 3.0 * L2 * a;
du = K2 * (H2 * (1.5 * a * a - 0.5) - 3.0 * L2 * a * a);
/* step1 in phase (degree 3) */
dp+=K3*L3*(7.5*a*a-1.5);
du+=K3*(H3*(2.5*a*a*a-1.5*a)-L3*(7.5*a*a-1.5)*a);
dp += K3 * L3 * (7.5 * a * a - 1.5);
du += K3 * (H3 * (2.5 * a * a * a - 1.5 * a) - L3 * (7.5 * a * a - 1.5) * a);
/* step1 out-of-phase (only radial) */
du+=3.0/4.0*0.0025*K2*sin(2.0*latp)*sin(2.0*pos[0])*sin(pos[1]-lonp);
du+=3.0/4.0*0.0022*K2*cosp*cosp*cosl*cosl*sin(2.0*(pos[1]-lonp));
du += 3.0 / 4.0 * 0.0025 * K2 * sin(2.0 * latp) * sin(2.0 * pos[0]) * sin(pos[1] - lonp);
du += 3.0 / 4.0 * 0.0022 * K2 * cosp * cosp * cosl * cosl * sin(2.0 * (pos[1] - lonp));
dr[0]=dp*ep[0]+du*eu[0];
dr[1]=dp*ep[1]+du*eu[1];
dr[2]=dp*ep[2]+du*eu[2];
dr[0] = dp * ep[0] + du * eu[0];
dr[1] = dp * ep[1] + du * eu[1];
dr[2] = dp * ep[2] + du * eu[2];
trace(5,"tide_pl : dr=%.3f %.3f %.3f\n",dr[0],dr[1],dr[2]);
trace(5, "tide_pl : dr=%.3f %.3f %.3f\n", dr[0], dr[1], dr[2]);
}
/* displacement by solid earth tide (ref [2] 7) ------------------------------*/
void tide_solid(const double *rsun, const double *rmoon,
const double *pos, const double *E, double gmst, int opt,
double *dr)
const double *pos, const double *E, double gmst, int opt,
double *dr)
{
double dr1[3],dr2[3],eu[3],du,dn,sinl,sin2l;
double dr1[3], dr2[3], eu[3], du, dn, sinl, sin2l;
trace(3,"tide_solid: pos=%.3f %.3f opt=%d\n",pos[0]*R2D,pos[1]*R2D,opt);
trace(3, "tide_solid: pos=%.3f %.3f opt=%d\n", pos[0] * R2D, pos[1] * R2D, opt);
/* step1: time domain */
eu[0]=E[2]; eu[1]=E[5]; eu[2]=E[8];
tide_pl(eu,rsun, GMS,pos,dr1);
tide_pl(eu,rmoon,GMM,pos,dr2);
eu[0] = E[2];
eu[1] = E[5];
eu[2] = E[8];
tide_pl(eu, rsun, GMS, pos, dr1);
tide_pl(eu, rmoon, GMM, pos, dr2);
/* step2: frequency domain, only K1 radial */
sin2l=sin(2.0*pos[0]);
du=-0.012*sin2l*sin(gmst+pos[1]);
sin2l = sin(2.0 * pos[0]);
du = -0.012 * sin2l * sin(gmst + pos[1]);
dr[0]=dr1[0]+dr2[0]+du*E[2];
dr[1]=dr1[1]+dr2[1]+du*E[5];
dr[2]=dr1[2]+dr2[2]+du*E[8];
dr[0] = dr1[0] + dr2[0] + du * E[2];
dr[1] = dr1[1] + dr2[1] + du * E[5];
dr[2] = dr1[2] + dr2[2] + du * E[8];
/* eliminate permanent deformation */
if (opt&8)
if (opt & 8)
{
sinl=sin(pos[0]);
du=0.1196*(1.5*sinl*sinl-0.5);
dn=0.0247*sin2l;
dr[0]+=du*E[2]+dn*E[1];
dr[1]+=du*E[5]+dn*E[4];
dr[2]+=du*E[8]+dn*E[7];
sinl = sin(pos[0]);
du = 0.1196 * (1.5 * sinl * sinl - 0.5);
dn = 0.0247 * sin2l;
dr[0] += du * E[2] + dn * E[1];
dr[1] += du * E[5] + dn * E[4];
dr[2] += du * E[8] + dn * E[7];
}
trace(5,"tide_solid: dr=%.3f %.3f %.3f\n",dr[0],dr[1],dr[2]);
trace(5, "tide_solid: dr=%.3f %.3f %.3f\n", dr[0], dr[1], dr[2]);
}
//#endif /* !IERS_MODEL */
@@ -139,99 +143,101 @@ void tide_solid(const double *rsun, const double *rmoon,
/* displacement by ocean tide loading (ref [2] 7) ----------------------------*/
void tide_oload(gtime_t tut, const double *odisp, double *denu)
{
const double args[][5]={
{1.40519E-4, 2.0,-2.0, 0.0, 0.00}, /* M2 */
{1.45444E-4, 0.0, 0.0, 0.0, 0.00}, /* S2 */
{1.37880E-4, 2.0,-3.0, 1.0, 0.00}, /* N2 */
{1.45842E-4, 2.0, 0.0, 0.0, 0.00}, /* K2 */
{0.72921E-4, 1.0, 0.0, 0.0, 0.25}, /* K1 */
{0.67598E-4, 1.0,-2.0, 0.0,-0.25}, /* O1 */
{0.72523E-4,-1.0, 0.0, 0.0,-0.25}, /* P1 */
{0.64959E-4, 1.0,-3.0, 1.0,-0.25}, /* Q1 */
{0.53234E-5, 0.0, 2.0, 0.0, 0.00}, /* Mf */
{0.26392E-5, 0.0, 1.0,-1.0, 0.00}, /* Mm */
{0.03982E-5, 2.0, 0.0, 0.0, 0.00} /* Ssa */
const double args[][5] = {
{1.40519E-4, 2.0, -2.0, 0.0, 0.00}, /* M2 */
{1.45444E-4, 0.0, 0.0, 0.0, 0.00}, /* S2 */
{1.37880E-4, 2.0, -3.0, 1.0, 0.00}, /* N2 */
{1.45842E-4, 2.0, 0.0, 0.0, 0.00}, /* K2 */
{0.72921E-4, 1.0, 0.0, 0.0, 0.25}, /* K1 */
{0.67598E-4, 1.0, -2.0, 0.0, -0.25}, /* O1 */
{0.72523E-4, -1.0, 0.0, 0.0, -0.25}, /* P1 */
{0.64959E-4, 1.0, -3.0, 1.0, -0.25}, /* Q1 */
{0.53234E-5, 0.0, 2.0, 0.0, 0.00}, /* Mf */
{0.26392E-5, 0.0, 1.0, -1.0, 0.00}, /* Mm */
{0.03982E-5, 2.0, 0.0, 0.0, 0.00} /* Ssa */
};
const double ep1975[]={1975,1,1,0,0,0};
double ep[6],fday,days,t,t2,t3,a[5],ang,dp[3]={0};
int i,j;
const double ep1975[] = {1975, 1, 1, 0, 0, 0};
double ep[6], fday, days, t, t2, t3, a[5], ang, dp[3] = {0};
int i, j;
trace(3,"tide_oload:\n");
trace(3, "tide_oload:\n");
/* angular argument: see subroutine arg.f for reference [1] */
time2epoch(tut,ep);
fday=ep[3]*3600.0+ep[4]*60.0+ep[5];
ep[3]=ep[4]=ep[5]=0.0;
days=timediff(epoch2time(ep),epoch2time(ep1975))/86400.0+1.0;
t=(27392.500528+1.000000035*days)/36525.0;
t2=t*t; t3=t2*t;
time2epoch(tut, ep);
fday = ep[3] * 3600.0 + ep[4] * 60.0 + ep[5];
ep[3] = ep[4] = ep[5] = 0.0;
days = timediff(epoch2time(ep), epoch2time(ep1975)) / 86400.0 + 1.0;
t = (27392.500528 + 1.000000035 * days) / 36525.0;
t2 = t * t;
t3 = t2 * t;
a[0]=fday;
a[1]=(279.69668+36000.768930485*t+3.03E-4*t2)*D2R; /* H0 */
a[2]=(270.434358+481267.88314137*t-0.001133*t2+1.9E-6*t3)*D2R; /* S0 */
a[3]=(334.329653+4069.0340329577*t-0.010325*t2-1.2E-5*t3)*D2R; /* P0 */
a[4]=2.0*PI;
a[0] = fday;
a[1] = (279.69668 + 36000.768930485 * t + 3.03E-4 * t2) * D2R; /* H0 */
a[2] = (270.434358 + 481267.88314137 * t - 0.001133 * t2 + 1.9E-6 * t3) * D2R; /* S0 */
a[3] = (334.329653 + 4069.0340329577 * t - 0.010325 * t2 - 1.2E-5 * t3) * D2R; /* P0 */
a[4] = 2.0 * PI;
/* displacements by 11 constituents */
for (i=0;i<11;i++)
for (i = 0; i < 11; i++)
{
ang=0.0;
for (j=0;j<5;j++) ang+=a[j]*args[i][j];
for (j=0;j<3;j++) dp[j]+=odisp[j+i*6]*cos(ang-odisp[j+3+i*6]*D2R);
ang = 0.0;
for (j = 0; j < 5; j++) ang += a[j] * args[i][j];
for (j = 0; j < 3; j++) dp[j] += odisp[j + i * 6] * cos(ang - odisp[j + 3 + i * 6] * D2R);
}
denu[0]=-dp[1];
denu[1]=-dp[2];
denu[2]= dp[0];
denu[0] = -dp[1];
denu[1] = -dp[2];
denu[2] = dp[0];
trace(5,"tide_oload: denu=%.3f %.3f %.3f\n",denu[0],denu[1],denu[2]);
trace(5, "tide_oload: denu=%.3f %.3f %.3f\n", denu[0], denu[1], denu[2]);
}
/* iers mean pole (ref [7] eq.7.25) ------------------------------------------*/
void iers_mean_pole(gtime_t tut, double *xp_bar, double *yp_bar)
{
const double ep2000[]={2000,1,1,0,0,0};
double y,y2,y3;
const double ep2000[] = {2000, 1, 1, 0, 0, 0};
double y, y2, y3;
y=timediff(tut,epoch2time(ep2000))/86400.0/365.25;
y = timediff(tut, epoch2time(ep2000)) / 86400.0 / 365.25;
if (y<3653.0/365.25)
if (y < 3653.0 / 365.25)
{ /* until 2010.0 */
y2=y*y; y3=y2*y;
*xp_bar= 55.974+1.8243*y+0.18413*y2+0.007024*y3; /* (mas) */
*yp_bar=346.346+1.7896*y-0.10729*y2-0.000908*y3;
y2 = y * y;
y3 = y2 * y;
*xp_bar = 55.974 + 1.8243 * y + 0.18413 * y2 + 0.007024 * y3; /* (mas) */
*yp_bar = 346.346 + 1.7896 * y - 0.10729 * y2 - 0.000908 * y3;
}
else
{ /* after 2010.0 */
*xp_bar= 23.513+7.6141*y; /* (mas) */
*yp_bar=358.891-0.6287*y;
{ /* after 2010.0 */
*xp_bar = 23.513 + 7.6141 * y; /* (mas) */
*yp_bar = 358.891 - 0.6287 * y;
}
}
/* displacement by pole tide (ref [7] eq.7.26) --------------------------------*/
void tide_pole(gtime_t tut, const double *pos, const double *erpv,
double *denu)
double *denu)
{
double xp_bar,yp_bar,m1,m2,cosl,sinl;
double xp_bar, yp_bar, m1, m2, cosl, sinl;
trace(3,"tide_pole: pos=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D);
trace(3, "tide_pole: pos=%.3f %.3f\n", pos[0] * R2D, pos[1] * R2D);
/* iers mean pole (mas) */
iers_mean_pole(tut,&xp_bar,&yp_bar);
iers_mean_pole(tut, &xp_bar, &yp_bar);
/* ref [7] eq.7.24 */
m1= erpv[0]/AS2R-xp_bar*1E-3; /* (as) */
m2=-erpv[1]/AS2R+yp_bar*1E-3;
m1 = erpv[0] / AS2R - xp_bar * 1E-3; /* (as) */
m2 = -erpv[1] / AS2R + yp_bar * 1E-3;
/* sin(2*theta) = sin(2*phi), cos(2*theta)=-cos(2*phi) */
cosl=cos(pos[1]);
sinl=sin(pos[1]);
denu[0]= 9E-3*sin(pos[0]) *(m1*sinl-m2*cosl); /* de= Slambda (m) */
denu[1]= -9E-3*cos(2.0*pos[0])*(m1*cosl+m2*sinl); /* dn=-Stheta (m) */
denu[2]=-33E-3*sin(2.0*pos[0])*(m1*cosl+m2*sinl); /* du= Sr (m) */
cosl = cos(pos[1]);
sinl = sin(pos[1]);
denu[0] = 9E-3 * sin(pos[0]) * (m1 * sinl - m2 * cosl); /* de= Slambda (m) */
denu[1] = -9E-3 * cos(2.0 * pos[0]) * (m1 * cosl + m2 * sinl); /* dn=-Stheta (m) */
denu[2] = -33E-3 * sin(2.0 * pos[0]) * (m1 * cosl + m2 * sinl); /* du= Sr (m) */
trace(5,"tide_pole : denu=%.3f %.3f %.3f\n",denu[0],denu[1],denu[2]);
trace(5, "tide_pole : denu=%.3f %.3f %.3f\n", denu[0], denu[1], denu[2]);
}
@@ -261,61 +267,61 @@ void tide_pole(gtime_t tut, const double *pos, const double *erpv,
* ver.2.4.0 does not use ocean loading and pole tide corrections
*-----------------------------------------------------------------------------*/
void tidedisp(gtime_t tutc, const double *rr, int opt, const erp_t *erp,
const double *odisp, double *dr)
const double *odisp, double *dr)
{
gtime_t tut;
double pos[2],E[9],drt[3],denu[3],rs[3],rm[3],gmst,erpv[5]={0};
double pos[2], E[9], drt[3], denu[3], rs[3], rm[3], gmst, erpv[5] = {0};
int i;
#ifdef IERS_MODEL
double ep[6],fhr;
int year,mon,day;
double ep[6], fhr;
int year, mon, day;
#endif
trace(3,"tidedisp: tutc=%s\n",time_str(tutc,0));
trace(3, "tidedisp: tutc=%s\n", time_str(tutc, 0));
if (erp) geterp(erp,tutc,erpv);
if (erp) geterp(erp, tutc, erpv);
tut=timeadd(tutc,erpv[2]);
tut = timeadd(tutc, erpv[2]);
dr[0]=dr[1]=dr[2]=0.0;
dr[0] = dr[1] = dr[2] = 0.0;
if (norm_rtk(rr,3)<=0.0) return;
if (norm_rtk(rr, 3) <= 0.0) return;
pos[0]=asin(rr[2]/norm_rtk(rr,3));
pos[1]=atan2(rr[1],rr[0]);
xyz2enu(pos,E);
pos[0] = asin(rr[2] / norm_rtk(rr, 3));
pos[1] = atan2(rr[1], rr[0]);
xyz2enu(pos, E);
if (opt&1)
if (opt & 1)
{ /* solid earth tides */
/* sun and moon position in ecef */
sunmoonpos(tutc,erpv,rs,rm,&gmst);
sunmoonpos(tutc, erpv, rs, rm, &gmst);
#ifdef IERS_MODEL
time2epoch(tutc,ep);
year=(int)ep[0];
mon =(int)ep[1];
day =(int)ep[2];
fhr =ep[3]+ep[4]/60.0+ep[5]/3600.0;
time2epoch(tutc, ep);
year = (int)ep[0];
mon = (int)ep[1];
day = (int)ep[2];
fhr = ep[3] + ep[4] / 60.0 + ep[5] / 3600.0;
/* call DEHANTTIDEINEL */
// dehanttideinel_((double *)rr,&year,&mon,&day,&fhr,rs,rm,drt);
#else
tide_solid(rs,rm,pos,E,gmst,opt,drt);
tide_solid(rs, rm, pos, E, gmst, opt, drt);
#endif
for (i=0;i<3;i++) dr[i]+=drt[i];
for (i = 0; i < 3; i++) dr[i] += drt[i];
}
if ((opt&2)&&odisp)
if ((opt & 2) && odisp)
{ /* ocean tide loading */
tide_oload(tut,odisp,denu);
matmul("TN",3,1,3,1.0,E,denu,0.0,drt);
for (i=0;i<3;i++) dr[i]+=drt[i];
tide_oload(tut, odisp, denu);
matmul("TN", 3, 1, 3, 1.0, E, denu, 0.0, drt);
for (i = 0; i < 3; i++) dr[i] += drt[i];
}
if ((opt&4)&&erp)
if ((opt & 4) && erp)
{ /* pole tide */
tide_pole(tut,pos,erpv,denu);
matmul("TN",3,1,3,1.0,E,denu,0.0,drt);
for (i=0;i<3;i++) dr[i]+=drt[i];
tide_pole(tut, pos, erpv, denu);
matmul("TN", 3, 1, 3, 1.0, E, denu, 0.0, drt);
for (i = 0; i < 3; i++) dr[i] += drt[i];
}
trace(5,"tidedisp: dr=%.3f %.3f %.3f\n",dr[0],dr[1],dr[2]);
trace(5, "tidedisp: dr=%.3f %.3f %.3f\n", dr[0], dr[1], dr[2]);
}

10
src/algorithms/libs/rtklib/rtklib_tides.h
View File

@@ -73,11 +73,11 @@ const double GMS = 1.327124E+20; /* sun gravitational constant */
const double GMM = 4.902801E+12; /* moon gravitational constant */
void tide_pl(const double *eu, const double *rp, double GMp,
const double *pos, double *dr);
const double *pos, double *dr);
void tide_solid(const double *rsun, const double *rmoon,
const double *pos, const double *E, double gmst, int opt,
double *dr);
const double *pos, const double *E, double gmst, int opt,
double *dr);
void tide_oload(gtime_t tut, const double *odisp, double *denu);
@@ -85,8 +85,8 @@ void iers_mean_pole(gtime_t tut, double *xp_bar, double *yp_bar);
void tide_pole(gtime_t tut, const double *pos, const double *erpv,
double *denu);
double *denu);
void tidedisp(gtime_t tutc, const double *rr, int opt, const erp_t *erp,
const double *odisp, double *dr);
const double *odisp, double *dr);
#endif

11
src/algorithms/libs/short_x2_to_cshort.cc
View File

@@ -40,10 +40,9 @@ short_x2_to_cshort_sptr make_short_x2_to_cshort()
}
short_x2_to_cshort::short_x2_to_cshort() : sync_block("short_x2_to_cshort",
gr::io_signature::make (2, 2, sizeof(short)),
gr::io_signature::make (1, 1, sizeof(lv_16sc_t))) // lv_8sc_t is a Volk's typedef for std::complex<signed char>
gr::io_signature::make(2, 2, sizeof(short)),
gr::io_signature::make(1, 1, sizeof(lv_16sc_t))) // lv_8sc_t is a Volk's typedef for std::complex<signed char>
{
const int alignment_multiple = volk_get_alignment() / sizeof(lv_16sc_t);
set_alignment(std::max(1, alignment_multiple));
@@ -51,8 +50,8 @@ short_x2_to_cshort::short_x2_to_cshort() : sync_block("short_x2_to_cshort",
int short_x2_to_cshort::work(int noutput_items,
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items)
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items)
{
const short *in0 = reinterpret_cast<const short *>(input_items[0]);
const short *in1 = reinterpret_cast<const short *>(input_items[1]);
@@ -60,7 +59,7 @@ int short_x2_to_cshort::work(int noutput_items,
// This could be put into a volk kernel
short real_part;
short imag_part;
for(int number = 0; number < noutput_items; number++)
for (int number = 0; number < noutput_items; number++)
{
// lv_cmake(r, i) defined at volk/volk_complex.h
real_part = *in0++;

5
src/algorithms/libs/short_x2_to_cshort.h
View File

@@ -48,12 +48,13 @@ class short_x2_to_cshort : public gr::sync_block
{
private:
friend short_x2_to_cshort_sptr make_short_x2_to_cshort();
public:
short_x2_to_cshort();
int work(int noutput_items,
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items);
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items);
};
#endif