mirrors
/
gnss-sdr
mirror of https://github.com/gnss-sdr/gnss-sdr
1
0
Fork 0
Code Issues Releases Wiki Activity
gnss-sdr/src/core/system_parameters/beidou_dnav_navigation_mess...

868 lines
39 KiB
C++
Raw Blame History

/*!
* \file beidou_dnav_navigation_message.cc
* \brief Implementation of a BeiDou D1 NAV Data message decoder as described
* in BeiDou ICD Version 2.1
*
* \author Sergi Segura, 2018. sergi.segura.munoz(at)gmail.com
* \author Damian Miralles, 2018. dmiralles2009@gmail.com
* -----------------------------------------------------------------------------
*
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
* This file is part of GNSS-SDR.
*
* Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors)
* SPDX-License-Identifier: GPL-3.0-or-later
*
* -----------------------------------------------------------------------------
*/
#include "beidou_dnav_navigation_message.h"
#include "gnss_satellite.h"
#include <cmath> // for cos, sin, fmod, sqrt, atan2, fabs, floor
#include <iostream> // for string, operator<<, cout, ostream
#include <limits> // for std::numeric_limits
#include <numeric> // for accumulate
Beidou_Dnav_Navigation_Message::Beidou_Dnav_Navigation_Message()
{
auto gnss_sat = Gnss_Satellite();
const std::string _system("Beidou");
for (uint32_t i = 1; i < 64; i++)
{
satelliteBlock[i] = gnss_sat.what_block(_system, i);
almanacHealth[i] = 0;
}
}
void Beidou_Dnav_Navigation_Message::print_beidou_word_bytes(uint32_t BEIDOU_word) const
{
std::cout << " Word =" << std::bitset<32>(BEIDOU_word) << '\n';
}
bool Beidou_Dnav_Navigation_Message::read_navigation_bool(
const std::bitset<BEIDOU_DNAV_SUBFRAME_DATA_BITS>& bits,
const std::vector<std::pair<int32_t, int32_t>>& parameter) const
{
bool value = bits[BEIDOU_DNAV_SUBFRAME_DATA_BITS - parameter[0].first];
return value;
}
uint64_t Beidou_Dnav_Navigation_Message::read_navigation_unsigned(
const std::bitset<BEIDOU_DNAV_SUBFRAME_DATA_BITS>& bits,
const std::vector<std::pair<int32_t, int32_t>>& parameter) const
{
uint64_t value = 0ULL;
for (const auto& param : parameter)
{
for (int32_t j = 0; j < param.second; j++)
{
value = (value << 1U) | static_cast<uint64_t>(bits[BEIDOU_DNAV_SUBFRAME_DATA_BITS - param.first - j]);
}
}
return value;
}
int64_t Beidou_Dnav_Navigation_Message::read_navigation_signed(
const std::bitset<BEIDOU_DNAV_SUBFRAME_DATA_BITS>& bits,
const std::vector<std::pair<int32_t, int32_t>>& parameter) const
{
int64_t value = bits[BEIDOU_DNAV_SUBFRAME_DATA_BITS - parameter[0].first] ? -1LL : 0LL;
for (const auto& param : parameter)
{
for (int32_t j = 0; j < param.second; j++)
{
value = (value << 1) | static_cast<int64_t>(bits[BEIDOU_DNAV_SUBFRAME_DATA_BITS - param.first - j]);
}
}
return value;
}
int32_t Beidou_Dnav_Navigation_Message::d1_subframe_decoder(std::string const& subframe)
{
const std::bitset<BEIDOU_DNAV_SUBFRAME_DATA_BITS> subframe_bits(subframe);
const auto subframe_ID = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_FRAID));
// Perform crc computation (tbd)
flag_crc_test = true;
// Decode all 5 sub-frames
switch (subframe_ID)
{
case 1: // --- It is subframe 1 ---
d_SOW_SF1 = static_cast<double>(read_navigation_unsigned(subframe_bits, D1_SOW));
d_SOW = d_SOW_SF1; // Set transmission time
i_SV_health = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_SAT_H1));
d_AODC = static_cast<double>(read_navigation_unsigned(subframe_bits, D1_AODC));
i_SV_accuracy = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_URAI)); // (20.3.3.3.1.3)
i_BEIDOU_week = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_WN));
d_Toc = static_cast<double>(read_navigation_unsigned(subframe_bits, D1_TOC));
d_Toc = d_Toc * D1_TOC_LSB;
d_TGD1 = static_cast<double>(read_navigation_signed(subframe_bits, D1_TGD1));
d_TGD1 = d_TGD1 * D1_TGD1_LSB;
d_TGD2 = static_cast<double>(read_navigation_signed(subframe_bits, D1_TGD2));
d_TGD2 = d_TGD2 * D1_TGD2_LSB;
d_alpha0 = static_cast<double>(read_navigation_signed(subframe_bits, D1_ALPHA0));
d_alpha0 = d_alpha0 * D1_ALPHA0_LSB;
d_alpha1 = static_cast<double>(read_navigation_signed(subframe_bits, D1_ALPHA1));
d_alpha1 = d_alpha1 * D1_ALPHA1_LSB;
d_alpha2 = static_cast<double>(read_navigation_signed(subframe_bits, D1_ALPHA2));
d_alpha2 = d_alpha2 * D1_ALPHA2_LSB;
d_alpha3 = static_cast<double>(read_navigation_signed(subframe_bits, D1_ALPHA3));
d_alpha3 = d_alpha3 * D1_ALPHA3_LSB;
d_beta0 = static_cast<double>(read_navigation_signed(subframe_bits, D1_BETA0));
d_beta0 = d_beta0 * D1_BETA0_LSB;
d_beta1 = static_cast<double>(read_navigation_signed(subframe_bits, D1_BETA1));
d_beta1 = d_beta1 * D1_BETA1_LSB;
d_beta2 = static_cast<double>(read_navigation_signed(subframe_bits, D1_BETA2));
d_beta2 = d_beta2 * D1_BETA2_LSB;
d_beta3 = static_cast<double>(read_navigation_signed(subframe_bits, D1_BETA3));
d_beta3 = d_beta3 * D1_BETA3_LSB;
d_A_f2 = static_cast<double>(read_navigation_signed(subframe_bits, D1_A2));
d_A_f2 = d_A_f2 * D1_A2_LSB;
d_A_f0 = static_cast<double>(read_navigation_signed(subframe_bits, D1_A0));
d_A_f0 = d_A_f0 * D1_A0_LSB;
d_A_f1 = static_cast<double>(read_navigation_signed(subframe_bits, D1_A1));
d_A_f1 = d_A_f1 * D1_A1_LSB;
d_AODE = static_cast<double>(read_navigation_unsigned(subframe_bits, D1_AODE));
// Set system flags for message reception
flag_d1_sf1 = true;
flag_iono_valid = true;
flag_new_SOW_available = true;
break;
case 2: // --- It is subframe 2 ---
d_SOW_SF2 = static_cast<double>(read_navigation_unsigned(subframe_bits, D1_SOW));
d_SOW = d_SOW_SF2; // Set transmission time
d_Delta_n = static_cast<double>(read_navigation_signed(subframe_bits, D1_DELTA_N));
d_Delta_n = d_Delta_n * D1_DELTA_N_LSB;
d_Cuc = static_cast<double>(read_navigation_signed(subframe_bits, D1_CUC));
d_Cuc = d_Cuc * D1_CUC_LSB;
d_M_0 = static_cast<double>(read_navigation_signed(subframe_bits, D1_M0));
d_M_0 = d_M_0 * D1_M0_LSB;
d_eccentricity = static_cast<double>(read_navigation_unsigned(subframe_bits, D1_E));
d_eccentricity = d_eccentricity * D1_E_LSB;
d_Cus = static_cast<double>(read_navigation_signed(subframe_bits, D1_CUS));
d_Cus = d_Cus * D1_CUS_LSB;
d_Crc = static_cast<double>(read_navigation_signed(subframe_bits, D1_CRC));
d_Crc = d_Crc * D1_CRC_LSB;
d_Crs = static_cast<double>(read_navigation_signed(subframe_bits, D1_CRS));
d_Crs = d_Crs * D1_CRS_LSB;
d_sqrt_A = static_cast<double>(read_navigation_unsigned(subframe_bits, D1_SQRT_A));
d_sqrt_A = d_sqrt_A * D1_SQRT_A_LSB;
d_Toe_sf2 = static_cast<double>(read_navigation_unsigned(subframe_bits, D1_TOE_SF2));
d_Toe_sf2 = static_cast<double>((static_cast<uint32_t>(d_Toe_sf2) << 15U));
// Set system flags for message reception
flag_d1_sf2 = true;
flag_new_SOW_available = true;
break;
case 3: // --- It is subframe 3 ---
d_SOW_SF3 = static_cast<double>(read_navigation_unsigned(subframe_bits, D1_SOW));
d_SOW = d_SOW_SF3; // Set transmission time
d_Toe_sf3 = static_cast<double>(read_navigation_unsigned(subframe_bits, D1_TOE_SF3));
d_i_0 = static_cast<double>(read_navigation_signed(subframe_bits, D1_I0));
d_i_0 = d_i_0 * D1_I0_LSB;
d_Cic = static_cast<double>(read_navigation_signed(subframe_bits, D1_CIC));
d_Cic = d_Cic * D1_CIC_LSB;
d_OMEGA_DOT = static_cast<double>(read_navigation_signed(subframe_bits, D1_OMEGA_DOT));
d_OMEGA_DOT = d_OMEGA_DOT * D1_OMEGA_DOT_LSB;
d_Cis = static_cast<double>(read_navigation_signed(subframe_bits, D1_CIS));
d_Cis = d_Cis * D1_CIS_LSB;
d_IDOT = static_cast<double>(read_navigation_signed(subframe_bits, D1_IDOT));
d_IDOT = d_IDOT * D1_IDOT_LSB;
d_OMEGA0 = static_cast<double>(read_navigation_signed(subframe_bits, D1_OMEGA0));
d_OMEGA0 = d_OMEGA0 * D1_OMEGA0_LSB;
d_OMEGA = static_cast<double>(read_navigation_signed(subframe_bits, D1_OMEGA));
d_OMEGA = d_OMEGA * D1_OMEGA_LSB;
// Set system flags for message reception
flag_d1_sf3 = true;
flag_new_SOW_available = true;
break;
case 4: // --- It is subframe 4 ---
d_SOW_SF4 = static_cast<double>(read_navigation_unsigned(subframe_bits, D1_SOW));
d_SOW = d_SOW_SF4; // Set transmission time
d_SQRT_A_ALMANAC = static_cast<double>(read_navigation_unsigned(subframe_bits, D1_SQRT_A_ALMANAC));
d_SQRT_A_ALMANAC = d_SQRT_A_ALMANAC * D1_SQRT_A_ALMANAC_LSB;
d_A1_ALMANAC = static_cast<double>(read_navigation_signed(subframe_bits, D1_A1_ALMANAC));
d_A1_ALMANAC = d_A1_ALMANAC * D1_A1_ALMANAC_LSB;
d_A0_ALMANAC = static_cast<double>(read_navigation_signed(subframe_bits, D1_A0_ALMANAC));
d_A0_ALMANAC = d_A0_ALMANAC * D1_A0_ALMANAC_LSB;
d_OMEGA0_ALMANAC = static_cast<double>(read_navigation_signed(subframe_bits, D1_OMEGA0_ALMANAC));
d_OMEGA0_ALMANAC = d_OMEGA0_ALMANAC * D1_OMEGA0_ALMANAC_LSB;
d_E_ALMANAC = static_cast<double>(read_navigation_unsigned(subframe_bits, D1_E));
d_E_ALMANAC = d_E_ALMANAC * D1_E_ALMANAC_LSB;
d_DELTA_I = static_cast<double>(read_navigation_signed(subframe_bits, D1_DELTA_I));
d_DELTA_I = D1_DELTA_I_LSB;
d_TOA = static_cast<double>(read_navigation_unsigned(subframe_bits, D1_TOA));
d_TOA = d_TOA * D1_TOA_LSB;
d_OMEGA_DOT_ALMANAC = static_cast<double>(read_navigation_signed(subframe_bits, D1_OMEGA_DOT_ALMANAC));
d_OMEGA_DOT_ALMANAC = D1_OMEGA_DOT_ALMANAC_LSB;
d_OMEGA_ALMANAC = static_cast<double>(read_navigation_signed(subframe_bits, D1_OMEGA_ALMANAC));
d_OMEGA_ALMANAC = d_OMEGA_ALMANAC * D1_OMEGA_ALMANAC_LSB;
d_M0_ALMANAC = static_cast<double>(read_navigation_signed(subframe_bits, D1_M0));
d_M0_ALMANAC = d_M0_ALMANAC * D1_M0_ALMANAC_LSB;
// Set system flags for message reception
flag_d1_sf4 = true;
flag_new_SOW_available = true;
break;
case 5: // --- It is subframe 5 ---
int32_t SV_page_5;
d_SOW_SF5 = static_cast<double>(read_navigation_unsigned(subframe_bits, D1_SOW));
d_SOW = d_SOW_SF5; // Set transmission time
SV_page_5 = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_PNUM));
if (SV_page_5 < 7)
{
d_SOW_SF4 = static_cast<double>(read_navigation_unsigned(subframe_bits, D1_SOW));
d_SQRT_A_ALMANAC = static_cast<double>(read_navigation_unsigned(subframe_bits, D1_SQRT_A_ALMANAC));
d_SQRT_A_ALMANAC = d_SQRT_A_ALMANAC * D1_SQRT_A_ALMANAC_LSB;
d_A1_ALMANAC = static_cast<double>(read_navigation_signed(subframe_bits, D1_A1_ALMANAC));
d_A1_ALMANAC = d_A1_ALMANAC * D1_A1_ALMANAC_LSB;
d_A0_ALMANAC = static_cast<double>(read_navigation_signed(subframe_bits, D1_A0_ALMANAC));
d_A0_ALMANAC = d_A0_ALMANAC * D1_A0_ALMANAC_LSB;
d_OMEGA0_ALMANAC = static_cast<double>(read_navigation_signed(subframe_bits, D1_OMEGA0_ALMANAC));
d_OMEGA0_ALMANAC = d_OMEGA0_ALMANAC * D1_OMEGA0_ALMANAC_LSB;
d_E_ALMANAC = static_cast<double>(read_navigation_unsigned(subframe_bits, D1_E));
d_E_ALMANAC = d_E_ALMANAC * D1_E_ALMANAC_LSB;
d_DELTA_I = static_cast<double>(read_navigation_signed(subframe_bits, D1_DELTA_I));
d_DELTA_I = D1_DELTA_I_LSB;
d_TOA = static_cast<double>(read_navigation_unsigned(subframe_bits, D1_TOA));
d_TOA = d_TOA * D1_TOA_LSB;
d_OMEGA_DOT_ALMANAC = static_cast<double>(read_navigation_signed(subframe_bits, D1_OMEGA_DOT_ALMANAC));
d_OMEGA_DOT_ALMANAC = D1_OMEGA_DOT_ALMANAC_LSB;
d_OMEGA_ALMANAC = static_cast<double>(read_navigation_signed(subframe_bits, D1_OMEGA_ALMANAC));
d_OMEGA_ALMANAC = d_OMEGA_ALMANAC * D1_OMEGA_ALMANAC_LSB;
d_M0_ALMANAC = static_cast<double>(read_navigation_signed(subframe_bits, D1_M0));
d_M0_ALMANAC = d_M0_ALMANAC * D1_M0_ALMANAC_LSB;
}
if (SV_page_5 == 7)
{
almanacHealth[1] = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_HEA1));
almanacHealth[2] = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_HEA2));
almanacHealth[3] = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_HEA3));
almanacHealth[4] = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_HEA4));
almanacHealth[5] = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_HEA5));
almanacHealth[6] = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_HEA6));
almanacHealth[7] = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_HEA7));
almanacHealth[8] = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_HEA8));
almanacHealth[9] = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_HEA9));
almanacHealth[10] = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_HEA10));
almanacHealth[11] = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_HEA11));
almanacHealth[12] = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_HEA12));
almanacHealth[13] = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_HEA13));
almanacHealth[14] = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_HEA14));
almanacHealth[15] = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_HEA15));
almanacHealth[16] = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_HEA16));
almanacHealth[17] = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_HEA17));
almanacHealth[18] = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_HEA18));
almanacHealth[19] = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_HEA19));
}
if (SV_page_5 == 8)
{
almanacHealth[20] = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_HEA20));
almanacHealth[21] = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_HEA21));
almanacHealth[22] = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_HEA22));
almanacHealth[23] = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_HEA23));
almanacHealth[24] = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_HEA24));
almanacHealth[25] = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_HEA25));
almanacHealth[26] = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_HEA26));
almanacHealth[27] = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_HEA27));
almanacHealth[28] = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_HEA28));
almanacHealth[29] = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_HEA29));
almanacHealth[30] = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_HEA30));
almanac_WN = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_WNA));
d_toa2 = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_TOA2));
}
if (SV_page_5 == 9)
{
d_A0GPS = static_cast<double>(read_navigation_signed(subframe_bits, D1_A0GPS)) * D1_A0GPS_LSB;
d_A1GPS = static_cast<double>(read_navigation_signed(subframe_bits, D1_A1GPS)) * D1_A1GPS_LSB;
d_A0GAL = static_cast<double>(read_navigation_signed(subframe_bits, D1_A0GAL)) * D1_A0GAL_LSB;
d_A1GAL = static_cast<double>(read_navigation_signed(subframe_bits, D1_A1GAL)) * D1_A1GAL_LSB;
d_A0GLO = static_cast<double>(read_navigation_signed(subframe_bits, D1_A0GLO)) * D1_A0GLO_LSB;
d_A1GLO = static_cast<double>(read_navigation_signed(subframe_bits, D1_A1GLO)) * D1_A1GLO_LSB;
flag_d1_sf5_p9 = true;
}
if (SV_page_5 == 10)
{
i_DeltaT_LS = static_cast<int>(read_navigation_signed(subframe_bits, D1_DELTA_T_LS));
d_DeltaT_LSF = static_cast<double>(read_navigation_signed(subframe_bits, D1_DELTA_T_LSF));
i_WN_LSF = static_cast<double>(read_navigation_signed(subframe_bits, D1_WN_LSF));
d_A0UTC = static_cast<double>(read_navigation_signed(subframe_bits, D1_A0UTC));
d_A0UTC = d_A0GPS * D1_A0GPS_LSB;
d_A1UTC = static_cast<double>(read_navigation_signed(subframe_bits, D1_A1UTC));
d_A1UTC = d_A1UTC * D1_A1UTC_LSB;
flag_d1_sf5_p10 = true;
}
// Set system flags for message reception
flag_d1_sf5 = true;
flag_new_SOW_available = true;
break;
default:
break;
} // switch subframeID ...
return subframe_ID;
}
int32_t Beidou_Dnav_Navigation_Message::d2_subframe_decoder(std::string const& subframe)
{
const std::bitset<BEIDOU_DNAV_SUBFRAME_DATA_BITS> subframe_bits(subframe);
const auto subframe_ID = static_cast<int>(read_navigation_unsigned(subframe_bits, D2_FRAID));
const auto page_ID = static_cast<int>(read_navigation_unsigned(subframe_bits, D2_PNUM));
// Perform crc computation (tbd)
flag_crc_test = true;
// Decode all 5 sub-frames
switch (subframe_ID)
{
// -- Decode the sub-frame id ------------------------------------------
case 1:
switch (page_ID)
{
case 1:
d_SOW = static_cast<double>(read_navigation_unsigned(subframe_bits, D2_SOW));
i_SV_health = static_cast<int>(read_navigation_unsigned(subframe_bits, D2_SAT_H1));
d_AODC = static_cast<double>(read_navigation_unsigned(subframe_bits, D2_AODC));
i_SV_accuracy = static_cast<int>(read_navigation_unsigned(subframe_bits, D2_URAI)); // (20.3.3.3.1.3)
i_BEIDOU_week = static_cast<int>(read_navigation_unsigned(subframe_bits, D2_WN));
d_Toc = static_cast<double>(read_navigation_unsigned(subframe_bits, D2_TOC)) * D1_TOC_LSB;
d_TGD1 = static_cast<double>(read_navigation_signed(subframe_bits, D2_TGD1)) * D1_TGD1_LSB;
// Set system flags for message reception
flag_sf1_p1 = true;
flag_new_SOW_available = true;
break;
case 2:
d_SOW = static_cast<double>(read_navigation_unsigned(subframe_bits, D2_SOW));
d_alpha0 = static_cast<double>(read_navigation_signed(subframe_bits, D2_ALPHA0)) * D1_ALPHA0_LSB;
d_alpha1 = static_cast<double>(read_navigation_signed(subframe_bits, D2_ALPHA1)) * D1_ALPHA1_LSB;
d_alpha2 = static_cast<double>(read_navigation_signed(subframe_bits, D2_ALPHA2)) * D1_ALPHA2_LSB;
d_alpha3 = static_cast<double>(read_navigation_signed(subframe_bits, D1_ALPHA3)) * D1_ALPHA3_LSB;
d_beta0 = static_cast<double>(read_navigation_signed(subframe_bits, D2_BETA0)) * D1_BETA0_LSB;
d_beta1 = static_cast<double>(read_navigation_signed(subframe_bits, D2_BETA1)) * D1_BETA1_LSB;
d_beta2 = static_cast<double>(read_navigation_signed(subframe_bits, D2_BETA2)) * D1_BETA2_LSB;
d_beta3 = static_cast<double>(read_navigation_signed(subframe_bits, D2_BETA3)) * D1_BETA3_LSB;
// Set system flags for message reception
flag_sf1_p2 = true;
flag_iono_valid = true;
flag_new_SOW_available = true;
break;
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_bits = (read_navigation_unsigned(subframe_bits, D2_A1_MSB));
// Adjust for lsb in next page
d_A_f1_msb_bits = d_A_f1_msb_bits << 18ULL;
// Set system flags for message reception
flag_sf1_p3 = true;
flag_new_SOW_available = true;
break;
case 4:
d_SOW = static_cast<double>(read_navigation_unsigned(subframe_bits, D2_SOW));
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_bits = (read_navigation_unsigned(subframe_bits, D2_CUC_MSB));
// Adjust for lsb in next page
d_Cuc_msb_bits = d_Cuc_msb_bits << 4U;
// Set system flags for message reception
flag_sf1_p4 = true;
flag_new_SOW_available = true;
break;
case 5:
d_SOW = static_cast<double>(read_navigation_unsigned(subframe_bits, D2_SOW));
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<uint64_t>(d_eccentricity_msb) << 22U;
d_eccentricity_msb_bits = d_eccentricity_msb_bits << 22U;
// Set system flags for message reception
flag_sf1_p5 = true;
flag_new_SOW_available = true;
break;
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_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_bits = d_Cic_msb_bits << 8U;
// Set system flags for message reception
flag_sf1_p6 = true;
flag_new_SOW_available = true;
break;
case 7:
d_SOW = static_cast<double>(read_navigation_unsigned(subframe_bits, D2_SOW));
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_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_bits = d_i_0_msb_bits << 11U;
// Set system flags for message reception
flag_sf1_p7 = true;
flag_new_SOW_available = true;
break;
case 8:
d_SOW = static_cast<double>(read_navigation_unsigned(subframe_bits, D2_SOW));
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_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_bits = d_OMEGA_DOT_msb_bits << 5ULL;
// Set system flags for message reception
flag_sf1_p8 = true;
flag_new_SOW_available = true;
break;
case 9:
d_SOW = static_cast<double>(read_navigation_unsigned(subframe_bits, D2_SOW));
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_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_bits = d_OMEGA_msb_bits << 5U;
// Set system flags for message reception
flag_sf1_p9 = true;
flag_new_SOW_available = true;
break;
case 10:
d_SOW = static_cast<double>(read_navigation_unsigned(subframe_bits, D2_SOW));
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
flag_sf1_p10 = true;
flag_new_SOW_available = true;
break;
default:
break;
}
break;
case 2: // -- It is subframe 2 -------------------
break;
case 3: // --- It is subframe 3 -------------------------------------
break;
case 4: // --- It is subframe 4 ---------- Almanac, ionospheric model, UTC parameters, SV health (PRN: 25-32)
break;
case 5: // -- It is subframe 5 -----------------almanac health (PRN: 1-24) and Almanac reference week number and time.
break;
default:
break;
} // switch subframeID ...
return subframe_ID;
}
double Beidou_Dnav_Navigation_Message::utc_time(double beidoutime_corrected) const
{
double t_utc;
double t_utc_daytime;
const double Delta_t_UTC = i_DeltaT_LS + d_A0UTC + d_A1UTC * (beidoutime_corrected);
// Determine if the effectivity time of the leap second event is in the past
const int32_t weeksToLeapSecondEvent = i_WN_LSF - i_BEIDOU_week;
if ((weeksToLeapSecondEvent) >= 0) // is not in the past
{
// Detect if the effectivity time and user's time is within six hours = 6 * 60 *60 = 21600 s
const int32_t secondOfLeapSecondEvent = i_DN * 24 * 60 * 60;
if (weeksToLeapSecondEvent > 0)
{
t_utc_daytime = fmod(beidoutime_corrected - Delta_t_UTC, 86400);
}
else // we are in the same week than the leap second event
{
if ((beidoutime_corrected - secondOfLeapSecondEvent) < (static_cast<double>(2) / static_cast<double>(3)) * 24 * 60 * 60)
{
t_utc_daytime = fmod(beidoutime_corrected - Delta_t_UTC, 86400);
}
else
{
if ((beidoutime_corrected - secondOfLeapSecondEvent) < (static_cast<double>(5) / static_cast<double>(4)) * 24 * 60 * 60)
{
const int32_t W = fmod(beidoutime_corrected - Delta_t_UTC - 43200, 86400) + 43200;
t_utc_daytime = fmod(W, 86400 + d_DeltaT_LSF - i_DeltaT_LS);
}
else
{
t_utc_daytime = fmod(beidoutime_corrected - Delta_t_UTC, 86400);
}
}
}
}
else // the effectivity time is in the past
{
t_utc_daytime = fmod(beidoutime_corrected - Delta_t_UTC, 86400);
}
const double secondsOfWeekBeforeToday = 43200 * floor(beidoutime_corrected / 43200);
t_utc = secondsOfWeekBeforeToday + t_utc_daytime;
return t_utc;
}
Beidou_Dnav_Ephemeris Beidou_Dnav_Navigation_Message::get_ephemeris() const
{
Beidou_Dnav_Ephemeris eph;
if ((i_satellite_PRN > 0 and i_satellite_PRN < 6) or i_satellite_PRN > 58)
{
std::bitset<BEIDOU_DNAV_SUBFRAME_DATA_BITS> subframe_bits;
// Order as given by eph_t in rtklib
eph.PRN = i_satellite_PRN;
eph.AODC = d_AODC;
eph.AODE = d_AODE;
eph.SV_accuracy = i_SV_accuracy;
eph.SV_health = i_SV_health;
eph.WN = i_BEIDOU_week;
eph.sig_type = i_signal_type;
eph.nav_type = 2;
eph.tow = d_SOW;
eph.toe = d_Toe;
eph.toc = d_Toc;
eph.sqrtA = d_sqrt_A;
eph.ecc = static_cast<double>((d_eccentricity_msb + d_eccentricity_lsb)) * D1_E_LSB;
subframe_bits = std::bitset<BEIDOU_DNAV_SUBFRAME_DATA_BITS>(d_i_0_msb_bits + d_i_0_lsb_bits);
eph.i_0 = static_cast<double>(read_navigation_signed(subframe_bits, D2_I0)) * D1_I0_LSB;
eph.OMEGA_0 = d_OMEGA0;
subframe_bits = std::bitset<BEIDOU_DNAV_SUBFRAME_DATA_BITS>(d_OMEGA_msb_bits + d_OMEGA_lsb_bits);
eph.omega = static_cast<double>(read_navigation_signed(subframe_bits, D2_OMEGA)) * D1_OMEGA_LSB;
eph.M_0 = d_M_0;
eph.delta_n = d_Delta_n;
subframe_bits = std::bitset<BEIDOU_DNAV_SUBFRAME_DATA_BITS>(d_OMEGA_DOT_msb_bits + d_OMEGA_DOT_lsb_bits);
eph.OMEGAdot = static_cast<double>(read_navigation_signed(subframe_bits, D2_OMEGA_DOT)) * D1_OMEGA_DOT_LSB;
eph.idot = d_IDOT;
eph.Crc = d_Crc;
eph.Crs = d_Crs;
subframe_bits = std::bitset<BEIDOU_DNAV_SUBFRAME_DATA_BITS>(d_Cuc_msb_bits + d_Cuc_lsb_bits);
eph.Cuc = static_cast<double>(read_navigation_signed(subframe_bits, D2_CUC)) * D1_CUC_LSB;
eph.Cus = d_Cus;
subframe_bits = std::bitset<BEIDOU_DNAV_SUBFRAME_DATA_BITS>(d_Cic_msb_bits + d_Cic_lsb_bits);
eph.Cic = static_cast<double>(read_navigation_signed(subframe_bits, D2_CIC)) * D1_CIC_LSB;
eph.Cis = d_Cis;
eph.af0 = d_A_f0;
subframe_bits = std::bitset<BEIDOU_DNAV_SUBFRAME_DATA_BITS>(d_A_f1_msb_bits + d_A_f1_lsb_bits);
eph.af1 = static_cast<double>(read_navigation_signed(subframe_bits, D2_A1)) * D1_A1_LSB;
eph.af2 = d_A_f2;
eph.TGD1 = d_TGD1;
eph.TGD2 = d_TGD2;
}
else
{
eph.PRN = i_satellite_PRN;
eph.AODC = d_AODC;
eph.AODE = d_AODE;
eph.SV_accuracy = i_SV_accuracy;
eph.SV_health = i_SV_health;
eph.WN = i_BEIDOU_week;
eph.sig_type = i_signal_type;
eph.nav_type = 1; // MEO/IGSO
eph.tow = d_SOW;
eph.toe = ((d_Toe_sf2 + d_Toe_sf3) * D1_TOE_LSB);
eph.toc = d_Toc;
eph.sqrtA = d_sqrt_A;
eph.ecc = d_eccentricity;
eph.i_0 = d_i_0;
eph.OMEGA_0 = d_OMEGA0;
eph.omega = d_OMEGA;
eph.M_0 = d_M_0;
eph.delta_n = d_Delta_n;
eph.OMEGAdot = d_OMEGA_DOT;
eph.idot = d_IDOT;
eph.Crc = d_Crc;
eph.Crs = d_Crs;
eph.Cuc = d_Cuc;
eph.Cus = d_Cus;
eph.Cic = d_Cic;
eph.Cis = d_Cis;
eph.af0 = d_A_f0;
eph.af1 = d_A_f1;
eph.af2 = d_A_f2;
eph.TGD1 = d_TGD1;
eph.TGD2 = d_TGD2;
}
return eph;
}
Beidou_Dnav_Iono Beidou_Dnav_Navigation_Message::get_iono()
{
Beidou_Dnav_Iono iono;
iono.alpha0 = d_alpha0;
iono.alpha1 = d_alpha1;
iono.alpha2 = d_alpha2;
iono.alpha3 = d_alpha3;
iono.beta0 = d_beta0;
iono.beta1 = d_beta1;
iono.beta2 = d_beta2;
iono.beta3 = d_beta3;
iono.valid = flag_iono_valid;
// WARNING: We clear flag_utc_model_valid in order to not re-send the same information to the ionospheric parameters queue
flag_iono_valid = false;
return iono;
}
Beidou_Dnav_Utc_Model Beidou_Dnav_Navigation_Message::get_utc_model()
{
Beidou_Dnav_Utc_Model utc_model;
utc_model.valid = flag_utc_model_valid;
// UTC parameters
utc_model.A1_UTC = d_A1UTC;
utc_model.A0_UTC = d_A0UTC;
utc_model.DeltaT_LS = i_DeltaT_LS;
utc_model.WN_LSF = i_WN_LSF;
utc_model.DN = i_DN;
utc_model.DeltaT_LSF = d_DeltaT_LSF;
utc_model.A0_GPS = d_A0GPS;
utc_model.A1_GPS = d_A1GPS;
utc_model.A0_GAL = d_A0GAL;
utc_model.A1_GAL = d_A1GAL;
utc_model.A0_GLO = d_A0GLO;
utc_model.A1_GLO = d_A1GLO;
// warning: We clear flag_utc_model_valid in order to not re-send the same information to the ionospheric parameters queue
flag_utc_model_valid = false;
return utc_model;
}
bool Beidou_Dnav_Navigation_Message::have_new_ephemeris() // Check if we have a new ephemeris stored in the galileo navigation class
{
if ((i_satellite_PRN > 0 and i_satellite_PRN < 6) or i_satellite_PRN > 58)
{
if ((flag_sf1_p1 == true) and (flag_sf1_p2 == true) and (flag_sf1_p3 == true) and
(flag_sf1_p4 == true) and (flag_sf1_p5 == true) and (flag_sf1_p6 == true) and
(flag_sf1_p7 == true) and (flag_sf1_p8 == true) and (flag_sf1_p9 == true) and
(flag_sf1_p10 == true))
{
// if all ephemeris pages have the same IOD, then they belong to the same block
if (d_previous_aode != d_AODE)
{
// Clear flags for all received pages
flag_sf1_p1 = false;
flag_sf1_p2 = false;
flag_sf1_p3 = false;
flag_sf1_p4 = false;
flag_sf1_p5 = false;
flag_sf1_p6 = false;
flag_sf1_p7 = false;
flag_sf1_p8 = false;
flag_sf1_p9 = false;
flag_sf1_p10 = false;
flag_eph_valid = true;
// Update the time of ephemeris information
d_previous_aode = d_AODE;
return true;
}
}
}
else
{
if ((flag_d1_sf1 == true) and (flag_d1_sf2 == true) and (flag_d1_sf3 == true))
{
// if all ephemeris pages have the same IOD, then they belong to the same block
if (d_previous_aode != d_AODE)
{
// Clear flags for all received subframes
flag_d1_sf1 = false;
flag_d1_sf2 = false;
flag_d1_sf3 = false;
flag_eph_valid = true;
// Update the time of ephemeris information
d_previous_aode = d_AODE;
return true;
}
}
}
return false;
}
bool Beidou_Dnav_Navigation_Message::have_new_iono() const
{
// the condition on flag_utc_model is added to have a time stamp for iono
if (flag_iono_valid == true)
{
return true;
}
return false;
}
bool Beidou_Dnav_Navigation_Message::have_new_utc_model()
{
if (flag_d1_sf5_p9 == true and flag_d1_sf5_p10 == true)
{
flag_d1_sf5_p9 = false;
flag_d1_sf5_p10 = false;
flag_utc_model_valid = true;
return true;
}
return false;
}
bool Beidou_Dnav_Navigation_Message::have_new_almanac()
{
if ((flag_d1_sf4 == true) and (flag_d1_sf5 == true))
{
// All Almanac data have been received
flag_d1_sf4 = false;
flag_d1_sf5 = false;
return true;
}
return false;
}
bool Beidou_Dnav_Navigation_Message::satellite_validation()
{
bool flag_data_valid = false;
flag_eph_valid = false;
// First Step:
// check Issue Of Ephemeris Data (AODE AODC..) to find a possible interrupted reception
// and check if the data have been filled (!=0)
if (d_SOW_SF1 != 0 and d_SOW_SF2 != 0 and d_SOW_SF3 != 0)
{
if (d_AODC != -1)
{
flag_data_valid = true;
flag_eph_valid = true;
}
}
return flag_data_valid;
}
Reference in New Issue View Git Blame Copy Permalink