mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-06-26 15:03:14 +00:00
Code Issues Releases Wiki Activity

bds_b1i: First BeiDou PVT for GNSS-SDR

Browse Source 
Adds a BeiDou B1I PVT for GNSS-SDR. Fixes bugs with D2 message decoding
for eph values in non continuous pages. Adds a couple of cosmetic fixes.
This commit is contained in:
Damian Miralles 2019-01-02 13:19:11 -06:00
parent b6b98bea69
commit 5395b0bc3b
4 changed files with 86 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/algorithms/PVT/libs/rinex_printer.cc
View File

@ -1642,7 +1642,7 @@ void Rinex_Printer::rinex_nav_header(std::fstream& out, const Beidou_Dnav_Iono&
line += std::string("BDUT");
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(utc_model.d_A0_UTC, 16, 2), 18);
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(utc_model.d_A1_UTC, 15, 2), 16);
line += std::string(23, ' ');
line += std::string(22, ' ');
line += Rinex_Printer::leftJustify("TIME SYSTEM CORR", 20);
Rinex_Printer::lengthCheck(line);
@ -2819,7 +2819,7 @@ void Rinex_Printer::update_nav_header(std::fstream& out, const Beidou_Dnav_Utc_M
line_aux += std::string("GPUT");
line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(utc_model.d_A0_UTC, 16, 2), 18);
line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(utc_model.d_A1_UTC, 15, 2), 16);
line_aux += std::string(23, ' ');
line_aux += std::string(22, ' ');
line_aux += Rinex_Printer::leftJustify("TIME SYSTEM CORR", 20);
data.push_back(line_aux);
}

18
src/core/system_parameters/Beidou_B1I.h
View File

@ -297,16 +297,18 @@ const std::vector<std::pair<int,int> > D2_BETA3( { {127,8} } );
// D2 NAV, SUBFRAME 1, PAGE 3
const std::vector<std::pair<int,int> > D2_A0( { {101,12},{121,12} } );
const std::vector<std::pair<int,int> > D2_A1_MSB( { {133,4} } );
const std::vector<std::pair<int,int> > D2_A1_LSB( { {47,6}, {61, 12} } );
const std::vector<std::pair<int,int> > D2_A1( { {279,22} } );
// D2 NAV, SUBFRAME 1, PAGE 4
const std::vector<std::pair<int,int> > D2_A1_LSB( { {47,6}, {61, 12} } );
const std::vector<std::pair<int,int> > D2_A2( { {73,10}, {91,1} } );
const std::vector<std::pair<int,int> > D2_AODE( { {92,5} } );
const std::vector<std::pair<int,int> > D2_DELTA_N( { {97,16} } );
const std::vector<std::pair<int,int> > D2_CUC_MSB( { {121,14} } );
const std::vector<std::pair<int,int> > D2_CUC_LSB( { {47,4} } );
const std::vector<std::pair<int,int> > D2_CUC( { {283,18} } );
// D2 NAV, SUBFRAME 1, PAGE 5
const std::vector<std::pair<int,int> > D2_CUC_LSB( { {47,4} } );
const std::vector<std::pair<int,int> > D2_M0( { {51,2}, {61,22}, {91,8} } );
const std::vector<std::pair<int,int> > D2_CUS( { {99,14}, {121, 4} } );
const std::vector<std::pair<int,int> > D2_E_MSB( { {125,10} } );
@ -315,26 +317,30 @@ const std::vector<std::pair<int,int> > D2_E_MSB( { {125,10} } );
const std::vector<std::pair<int,int> > D2_E_LSB( { {47,6}, {61, 16} } );
const std::vector<std::pair<int,int> > D2_SQRT_A( { {77,6},{91,22}, {121,4} } );
const std::vector<std::pair<int,int> > D2_CIC_MSB( { {125,10} } );
const std::vector<std::pair<int,int> > D2_CIC_LSB( { {47,6}, {61,2} } );
const std::vector<std::pair<int,int> > D2_CIC( { {283,18} } );
// D2 NAV, SUBFRAME 1, PAGE 7
const std::vector<std::pair<int,int> > D2_CIC_LSB( { {47,6}, {61,2} } );
const std::vector<std::pair<int,int> > D2_CIS( { {63,18} } );
const std::vector<std::pair<int,int> > D2_TOE( { {81,2},{91,15} } );
const std::vector<std::pair<int,int> > D2_I0_MSB( { {106,7},{121,14} } );
const std::vector<std::pair<int,int> > D2_I0_LSB( { {47,6},{61,5} } );
const std::vector<std::pair<int,int> > D2_I0( { {269,32} } );
// D2 NAV, SUBFRAME 1, PAGE 8
const std::vector<std::pair<int,int> > D2_I0_LSB( { {47,6},{61,5} } );
const std::vector<std::pair<int,int> > D2_CRC( { {66,17},{91,1} } );
const std::vector<std::pair<int,int> > D2_CRS( { {92,18} } );
const std::vector<std::pair<int,int> > D2_OMEGA_DOT_MSB( { {110,3},{121,16} } );
const std::vector<std::pair<int,int> > D2_OMEGA_DOT_LSB( { {47,5} } );
const std::vector<std::pair<int,int> > D2_OMEGA_DOT( { {277,24} } );
// D2 NAV, SUBFRAME 1, PAGE 9
const std::vector<std::pair<int,int> > D2_OMEGA_DOT_LSB( { {47,5} } );
const std::vector<std::pair<int,int> > D2_OMEGA0( { {52,1},{61,22},{91,9} } );
const std::vector<std::pair<int,int> > D2_OMEGA_MSB( { {100,13},{121,14} } );
const std::vector<std::pair<int,int> > D2_OMEGA_LSB( { {47,5} } );
const std::vector<std::pair<int,int> > D2_OMEGA( { {269,32} } );
// D2 NAV, SUBFRAME 1, PAGE 10
const std::vector<std::pair<int,int> > D2_OMEGA_LSB( { {47,5} } );
const std::vector<std::pair<int,int> > D2_IDOT( { {52,1},{61,13} } );
#endif /* GNSS_SDR_BEIDOU_B1I_H_ */

93
src/core/system_parameters/beidou_dnav_navigation_message.cc
View File

@ -33,6 +33,8 @@
#include <cmath>
#include <iostream>
#include <cstring>
#include <string>
#include <boost/crc.hpp> // for boost::crc_basic, boost::crc_optimal
#include <boost/dynamic_bitset.hpp>
#include <glog/logging.h>
@ -71,20 +73,24 @@ void Beidou_Dnav_Navigation_Message::reset()
flag_sf1_p10 = false;
// D2 NAV Decoding UNique Attributes
d_A_f1_msb = 0;
d_A_f1_lsb = 0;
d_Cuc_msb = 0;
d_Cuc_lsb = 0;
d_A_f1_msb_bits = 0;
d_A_f1_lsb_bits = 0;
d_Cuc_msb_bits = 0;
d_Cuc_lsb_bits = 0;
d_eccentricity_msb_bits = 0;
d_eccentricity_lsb_bits = 0;
d_Cic_msb_bits = 0;
d_Cic_lsb_bits = 0;
d_i_0_msb_bits = 0;
d_i_0_lsb_bits = 0;
d_OMEGA_msb_bits = 0;
d_OMEGA_lsb_bits = 0;
d_OMEGA_DOT_msb_bits = 0;
d_OMEGA_DOT_lsb_bits = 0;
// D2 NAV Decoding UNique Attributes
d_eccentricity_msb = 0;
d_eccentricity_lsb = 0;
d_Cic_msb = 0;
d_Cic_lsb = 0;
d_i_0_msb = 0;
d_i_0_lsb = 0;
d_OMEGA_msb = 0;
d_OMEGA_lsb = 0;
d_OMEGA_DOT_msb = 0;
d_OMEGA_DOT_lsb = 0;
d_SOW = 0;
d_SOW_SF1 = 0;
@ -783,9 +789,9 @@ int Beidou_Dnav_Navigation_Message::d2_subframe_decoder(std::string const &subfr
case 3:
d_SOW = static_cast<double>(read_navigation_unsigned(subframe_bits, D2_SOW));
d_A_f0 = static_cast<double>(read_navigation_signed(subframe_bits, D2_A0))*D1_A0_LSB;
d_A_f1_msb = static_cast<double>(read_navigation_signed(subframe_bits, D2_A1_MSB));
d_A_f1_msb_bits = (read_navigation_unsigned(subframe_bits, D2_A1_MSB));
// Adjust for lsb in next page
d_A_f1_msb = static_cast<double>((static_cast<int>(d_A_f1_msb) << 18));
d_A_f1_msb_bits = d_A_f1_msb_bits << 18;
// Set system flags for message reception
flag_sf1_p3 = true;
@ -794,13 +800,13 @@ int Beidou_Dnav_Navigation_Message::d2_subframe_decoder(std::string const &subfr
break;
case 4:
d_SOW = static_cast<double>(read_navigation_unsigned(subframe_bits, D2_SOW));
d_A_f1_lsb = static_cast<double>(read_navigation_signed(subframe_bits, D2_A1_LSB));
d_A_f1_lsb_bits = (read_navigation_unsigned(subframe_bits, D2_A1_LSB));
d_A_f2 = static_cast<double>(read_navigation_signed(subframe_bits, D1_A2))*D1_A2_LSB;
d_AODE = static_cast<double>(read_navigation_unsigned(subframe_bits, D2_AODE));
d_Delta_n = static_cast<double>(read_navigation_signed(subframe_bits, D2_DELTA_N))*D1_DELTA_N_LSB;
d_Cuc_msb = static_cast<double>(read_navigation_signed(subframe_bits, D2_CUC_MSB));
d_Cuc_msb_bits = (read_navigation_unsigned(subframe_bits, D2_CUC_MSB));
// Adjust for lsb in next page
d_Cuc_msb = static_cast<double>((static_cast<int>(d_Cuc_msb) << 4));
d_Cuc_msb_bits = d_Cuc_msb_bits << 4;
// Set system flags for message reception
flag_sf1_p4 = true;
@ -809,12 +815,14 @@ int Beidou_Dnav_Navigation_Message::d2_subframe_decoder(std::string const &subfr
break;
case 5:
d_SOW = static_cast<double>(read_navigation_unsigned(subframe_bits, D2_SOW));
d_Cuc_lsb = static_cast<double>(read_navigation_signed(subframe_bits, D2_CUC_LSB));
d_Cuc_lsb_bits = (read_navigation_unsigned(subframe_bits, D2_CUC_LSB));
d_M_0 = static_cast<double>(read_navigation_signed(subframe_bits, D2_M0))*D1_M0_LSB;
d_Cus = static_cast<double>(read_navigation_signed(subframe_bits, D2_CUS))*D1_CUS_LSB;
d_eccentricity_msb = static_cast<double>(read_navigation_unsigned(subframe_bits, D2_E_MSB));
d_eccentricity_msb_bits = (read_navigation_unsigned(subframe_bits, D2_E_MSB));
// Adjust for lsb in next page (shift number of lsb to the left)
d_eccentricity_msb = static_cast<double>((static_cast<int>(d_eccentricity_msb) << 22));
d_eccentricity_msb_bits = d_eccentricity_msb_bits << 22;
// Set system flags for message reception
flag_sf1_p5 = true;
@ -824,10 +832,11 @@ int Beidou_Dnav_Navigation_Message::d2_subframe_decoder(std::string const &subfr
case 6:
d_SOW = static_cast<double>(read_navigation_unsigned(subframe_bits, D2_SOW));
d_eccentricity_lsb = static_cast<double>(read_navigation_unsigned(subframe_bits, D2_E_LSB));
d_eccentricity_lsb_bits = (read_navigation_unsigned(subframe_bits, D2_E_LSB));
d_sqrt_A = static_cast<double>(read_navigation_unsigned(subframe_bits, D2_SQRT_A))*D1_SQRT_A_LSB;
d_Cic_msb = static_cast<double>(read_navigation_signed(subframe_bits, D2_CIC_MSB));
d_Cic_msb_bits = (read_navigation_unsigned(subframe_bits, D2_CIC_MSB));
// Adjust for lsb in next page (shift number of lsb to the left)
d_Cic_msb = static_cast<double>((static_cast<int>(d_Cic_msb) << 8));
d_Cic_msb_bits = d_Cic_msb_bits << 8;
// Set system flags for message reception
flag_sf1_p6 = true;
@ -836,12 +845,12 @@ int Beidou_Dnav_Navigation_Message::d2_subframe_decoder(std::string const &subfr
break;
case 7:
d_SOW = static_cast<double>(read_navigation_unsigned(subframe_bits, D2_SOW));
d_Cic_lsb = static_cast<double>(read_navigation_signed(subframe_bits, D2_CIC_LSB));
d_Cic_lsb_bits = (read_navigation_unsigned(subframe_bits, D2_CIC_LSB));
d_Cis = static_cast<double>(read_navigation_signed(subframe_bits, D2_CIS))*D1_CIS_LSB;
d_Toe = static_cast<double>(read_navigation_unsigned(subframe_bits, D2_TOE))*D1_TOE_LSB;
d_i_0_msb = static_cast<double>(read_navigation_signed(subframe_bits, D2_I0_MSB));
d_i_0_msb_bits = (read_navigation_unsigned(subframe_bits, D2_I0_MSB));
// Adjust for lsb in next page (shift number of lsb to the left)
d_i_0_msb = static_cast<double>((static_cast<int>(d_i_0_msb) << 11));
d_i_0_msb_bits = d_i_0_msb_bits << 11;
// Set system flags for message reception
flag_sf1_p7 = true;
@ -850,12 +859,12 @@ int Beidou_Dnav_Navigation_Message::d2_subframe_decoder(std::string const &subfr
break;
case 8:
d_SOW = static_cast<double>(read_navigation_unsigned(subframe_bits, D2_SOW));
d_i_0_lsb = static_cast<double>(read_navigation_signed(subframe_bits, D2_I0_LSB));
d_i_0_lsb_bits = (read_navigation_unsigned(subframe_bits, D2_I0_LSB));
d_Crc = static_cast<double>(read_navigation_signed(subframe_bits, D2_CRC))*D1_CRC_LSB;
d_Crs = static_cast<double>(read_navigation_signed(subframe_bits, D2_CRS))*D1_CRS_LSB;
d_OMEGA_DOT_msb = static_cast<double>(read_navigation_signed(subframe_bits, D2_OMEGA_DOT_MSB));
d_OMEGA_DOT_msb_bits = (read_navigation_unsigned(subframe_bits, D2_OMEGA_DOT_MSB));
// Adjust for lsb in next page (shift number of lsb to the left)
d_OMEGA_DOT_msb = static_cast<double>((static_cast<int>(d_OMEGA_DOT_msb) << 5));
d_OMEGA_DOT_msb_bits = d_OMEGA_DOT_msb_bits << 5;
// Set system flags for message reception
flag_sf1_p8 = true;
@ -864,11 +873,11 @@ int Beidou_Dnav_Navigation_Message::d2_subframe_decoder(std::string const &subfr
break;
case 9:
d_SOW = static_cast<double>(read_navigation_unsigned(subframe_bits, D2_SOW));
d_OMEGA_DOT_lsb = static_cast<double>(read_navigation_signed(subframe_bits, D2_OMEGA_DOT_LSB));
d_OMEGA_DOT_lsb_bits = (read_navigation_unsigned(subframe_bits, D2_OMEGA_DOT_LSB));
d_OMEGA0 = static_cast<double>(read_navigation_signed(subframe_bits, D2_OMEGA0))*D1_OMEGA0_LSB;
d_OMEGA_msb = static_cast<double>(read_navigation_signed(subframe_bits, D2_OMEGA_MSB));
d_OMEGA_msb_bits = (read_navigation_unsigned(subframe_bits, D2_OMEGA_MSB));
// Adjust for lsb in next page (shift number of lsb to the left)
d_OMEGA_msb = static_cast<double>((static_cast<int>(d_OMEGA_msb) << 5));
d_OMEGA_msb_bits = d_OMEGA_msb_bits << 5;
// Set system flags for message reception
flag_sf1_p9 = true;
@ -877,7 +886,7 @@ int Beidou_Dnav_Navigation_Message::d2_subframe_decoder(std::string const &subfr
break;
case 10:
d_SOW = static_cast<double>(read_navigation_unsigned(subframe_bits, D2_SOW));
d_OMEGA_lsb = static_cast<double>(read_navigation_unsigned(subframe_bits, D2_OMEGA_LSB));
d_OMEGA_lsb_bits = (read_navigation_unsigned(subframe_bits, D2_OMEGA_LSB));
d_IDOT = static_cast<double>(read_navigation_signed(subframe_bits, D2_IDOT))*D1_IDOT_LSB;
// Set system flags for message reception
@ -967,6 +976,8 @@ Beidou_Dnav_Ephemeris Beidou_Dnav_Navigation_Message::get_ephemeris()
if(i_satellite_PRN > 0 and i_satellite_PRN < 6)
{
std::bitset<BEIDOU_DNAV_SUBFRAME_DATA_BITS> subframe_bits;
// Order as given by eph_t in rtklib
eph.i_satellite_PRN = i_satellite_PRN;
eph.d_AODC = d_AODC;
@ -983,27 +994,35 @@ Beidou_Dnav_Ephemeris Beidou_Dnav_Navigation_Message::get_ephemeris()
eph.d_sqrt_A = d_sqrt_A;
eph.d_eccentricity = (d_eccentricity_msb + d_eccentricity_lsb)*D1_E_LSB;
eph.d_i_0 = (d_i_0_msb + d_i_0_lsb)*D1_I0_LSB;
subframe_bits = std::bitset<BEIDOU_DNAV_SUBFRAME_DATA_BITS>(d_i_0_msb_bits + d_i_0_lsb_bits);
eph.d_i_0 = static_cast<double>(read_navigation_signed(subframe_bits, D2_I0))*D1_I0_LSB;
eph.d_OMEGA0 = d_OMEGA0;
d_OMEGA_lsb = (d_OMEGA_msb < 0)?-d_OMEGA_lsb:d_OMEGA_lsb;
eph.d_OMEGA = (d_OMEGA_msb + d_OMEGA_lsb)*D1_OMEGA_LSB;
subframe_bits = std::bitset<BEIDOU_DNAV_SUBFRAME_DATA_BITS>(d_OMEGA_msb_bits + d_OMEGA_lsb_bits);
eph.d_OMEGA = static_cast<double>(read_navigation_signed(subframe_bits, D2_OMEGA))*D1_OMEGA_LSB;
eph.d_M_0 = d_M_0;
eph.d_Delta_n = d_Delta_n;
eph.d_OMEGA_DOT = (d_OMEGA_DOT_msb + d_OMEGA_DOT_lsb)*D1_OMEGA_DOT_LSB;
subframe_bits = std::bitset<BEIDOU_DNAV_SUBFRAME_DATA_BITS>(d_OMEGA_DOT_msb_bits + d_OMEGA_DOT_lsb_bits);
eph.d_OMEGA_DOT = static_cast<double>(read_navigation_signed(subframe_bits, D2_OMEGA_DOT))*D1_OMEGA_DOT_LSB;
eph.d_IDOT = d_IDOT;
eph.d_Crc = d_Crc;
eph.d_Crs = d_Crs;
eph.d_Cuc = (d_Cuc_msb + d_Cuc_lsb)*D1_CUC_LSB;
subframe_bits = std::bitset<BEIDOU_DNAV_SUBFRAME_DATA_BITS>(d_Cuc_msb_bits + d_Cuc_lsb_bits);
eph.d_Cuc = static_cast<double>(read_navigation_signed(subframe_bits, D2_CUC))*D1_CUC_LSB;
eph.d_Cus = d_Cus;
eph.d_Cic = (d_Cic_msb + d_Cic_lsb)*D1_CIC_LSB;
subframe_bits = std::bitset<BEIDOU_DNAV_SUBFRAME_DATA_BITS>(d_Cic_msb_bits + d_Cic_lsb_bits);
eph.d_Cic = static_cast<double>(read_navigation_signed(subframe_bits, D2_CIC))*D1_CIC_LSB;
eph.d_Cis = d_Cis;
eph.d_A_f0 = d_A_f0;
eph.d_A_f1 = (d_A_f1_msb + d_A_f1_lsb)*D1_A1_LSB;
subframe_bits = std::bitset<BEIDOU_DNAV_SUBFRAME_DATA_BITS>(d_A_f1_msb_bits + d_A_f1_lsb_bits);
eph.d_A_f1 = static_cast<double>(read_navigation_signed(subframe_bits, D2_A1))*D1_A1_LSB;
eph.d_A_f2 = d_A_f2;
eph.d_TGD1 = d_TGD1;
eph.d_TGD2 = d_TGD2;
}

30
src/core/system_parameters/beidou_dnav_navigation_message.h
View File

@ -150,20 +150,22 @@ public:
double d_A_f2; //!< Clock correction parameters. Coefficient 2 of code phase offset model [s/s^2]
// D2 NAV Message Decoding
double d_A_f1_msb; //!< Clock correction parameters, D2 NAV MSB
double d_A_f1_lsb; //!< Clock correction parameters, D2 NAV LSB
double d_Cuc_msb; //!< Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad]
double d_Cuc_lsb; //!< Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad]
double d_eccentricity_msb; //!< Eccentricity [dimensionless]
double d_eccentricity_lsb; //!< Eccentricity [dimensionless]
double d_Cic_msb; //!< Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad]
double d_Cic_lsb; //!< Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad]
double d_i_0_msb; //!< Inclination Angle at Reference Time [semi-circles]
double d_i_0_lsb; //!< Inclination Angle at Reference Time [semi-circles]
double d_OMEGA_msb; //!< Argument of Perigee [semi-cicles]
double d_OMEGA_lsb; //!< Argument of Perigee [semi-cicles]
double d_OMEGA_DOT_msb; //!< Rate of Right Ascension [semi-circles/s]
double d_OMEGA_DOT_lsb; //!< Rate of Right Ascension [semi-circles/s]
unsigned long int d_A_f1_msb_bits; //!< Clock correction parameters, D2 NAV MSB
unsigned long int d_A_f1_lsb_bits; //!< Clock correction parameters, D2 NAV LSB
unsigned long int d_Cuc_msb_bits; //!< Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad]
unsigned long int d_Cuc_lsb_bits; //!< Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad]
unsigned long int d_eccentricity_msb; //!< Eccentricity [dimensionless]
unsigned long int d_eccentricity_lsb; //!< Eccentricity [dimensionless]
unsigned long int d_Cic_msb_bits; //!< Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad]
unsigned long int d_Cic_lsb_bits; //!< Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad]
unsigned long int d_eccentricity_msb_bits; //!< Eccentricity [dimensionless]
unsigned long int d_eccentricity_lsb_bits;
unsigned long int d_i_0_msb_bits; //!< Inclination Angle at Reference Time [semi-circles]
unsigned long int d_i_0_lsb_bits; //!< Inclination Angle at Reference Time [semi-circles]
unsigned long int d_OMEGA_msb_bits; //!< Argument of Perigee [semi-cicles]
unsigned long int d_OMEGA_lsb_bits; //!< Argument of Perigee [semi-cicles]
unsigned long int d_OMEGA_DOT_msb_bits; //!< Rate of Right Ascension [semi-circles/s]
unsigned long int d_OMEGA_DOT_lsb_bits; //!< Rate of Right Ascension [semi-circles/s]
// Almanac
double d_Toa; //!< Almanac reference time [s]