From 0ac533b66973786a9954bc5a4b8275779df78e07 Mon Sep 17 00:00:00 2001 From: Jonathan McDermid Date: Thu, 24 Oct 2024 12:52:32 -0600 Subject: [PATCH 1/6] refactor rangecmp --- .../decoders/oem/rangecmp/common.hpp | 255 ++--- .../oem/rangecmp/range_decompressor.hpp | 77 +- .../oem/src/rangecmp/range_decompressor.cpp | 1018 ++++++++--------- src/decoders/oem/test/range_cmp_test.cpp | 118 +- 4 files changed, 665 insertions(+), 803 deletions(-) diff --git a/include/novatel_edie/decoders/oem/rangecmp/common.hpp b/include/novatel_edie/decoders/oem/rangecmp/common.hpp index 8784ec355..07270e563 100644 --- a/include/novatel_edie/decoders/oem/rangecmp/common.hpp +++ b/include/novatel_edie/decoders/oem/rangecmp/common.hpp @@ -38,8 +38,6 @@ namespace novatel::edie::oem { constexpr uint32_t SPEED_OF_LIGHT = 299792458; constexpr uint32_t MAX_VALUE = 0x800000; //!< Also 8388608, defined in RANGECMP documentation for ADR. constexpr uint32_t BITS_PER_BYTE = 8; -constexpr int32_t MAGIC_NEGATE = -1; //!< Some fields are magically negated in RANGE messages when - //!< translating from a compressed version. //! NOTE: See documentation on slot/PRN offsets for specific satellite systems: //! https://docs.novatel.com/OEM7/Content/Logs/RANGECMP4.htm @@ -169,7 +167,7 @@ constexpr uint32_t CTS_CHANNEL_ASSIGNMENT_SHIFT = 31; //----------------------------------------------------------------------- //! RANGECMP data field masks, shifts and scale factors. -//! NOTE: RangeCmpDataStruct defines a number of fields that can be +//! NOTE: RangeCmpData defines a number of fields that can be //! masked out from larger data types. //----------------------------------------------------------------------- constexpr uint64_t RC_DOPPLER_FREQUENCY_MASK = 0x000000000FFFFFFF; @@ -312,7 +310,6 @@ constexpr uint64_t RC4_SSIG_RBLK_PSR_SIGNEXT_MASK = 0xFFFFFFFFFFF00000; constexpr uint32_t RC4_RBLK_PHASERANGE_SIGNBIT_MASK = 0x00400000; constexpr uint32_t RC4_RBLK_PHASERANGE_SIGNEXT_MASK = 0xFF800000; -// PRIMARY SECONDARY constexpr std::array RC4_DBLK_INVALID_DOPPLER = {-131072, -8192}; constexpr std::array RC4_DBLK_DOPPLER_BITS = {18, 14}; constexpr std::array RC4_DBLK_DOPPLER_SIGNBIT_MASK = {0x00020000, 0x00002000}; @@ -347,10 +344,10 @@ enum class SYSTEM #pragma pack(push, 1) //----------------------------------------------------------------------- -//! \struct RangeDataStruct +//! \struct RangeData //! \brief Range data entry from a OEM4 binary RANGE message. //----------------------------------------------------------------------- -struct RangeDataStruct +struct RangeData { uint16_t usPRN{0}; int16_t sGLONASSFrequency{0}; @@ -363,27 +360,27 @@ struct RangeDataStruct float fLockTime{0}; uint32_t uiChannelTrackingStatus{0}; - constexpr RangeDataStruct() = default; + RangeData() = default; }; //----------------------------------------------------------------------- -//! \struct RangeStruct +//! \struct Range //! \brief OEM4 binary RANGE message representation. //----------------------------------------------------------------------- -struct RangeStruct +struct Range { uint32_t uiNumberOfObservations{0}; - RangeDataStruct astRangeData[RANGE_RECORD_MAX]{}; + RangeData astRangeData[RANGE_RECORD_MAX]{}; - constexpr RangeStruct() = default; + Range() = default; }; //----------------------------------------------------------------------- -//! \struct RangeCmpDataStruct +//! \struct RangeCmpData //! \brief Compressed range data entry from a OEM4 binary RANGECMP //! message. //----------------------------------------------------------------------- -struct RangeCmpDataStruct +struct RangeCmpData { uint32_t uiChannelTrackingStatus{0}; uint64_t ulDopplerFrequencyPSRField{0}; // This is a combination of two fields; Doppler Frequency and PSR. @@ -394,59 +391,59 @@ struct RangeCmpDataStruct // (5b), and GLONASS Frequency number (8b) uint16_t usReserved{0}; - constexpr RangeCmpDataStruct() = default; + RangeCmpData() = default; }; //----------------------------------------------------------------------- -//! \struct RangeCmpStruct +//! \struct RangeCmp //! \brief OEM4 binary RANGECMP message representation. //----------------------------------------------------------------------- -struct RangeCmpStruct +struct RangeCmp { uint32_t uiNumberOfObservations{0}; - RangeCmpDataStruct astRangeData[RANGE_RECORD_MAX]{}; + RangeCmpData astRangeData[RANGE_RECORD_MAX]{}; - constexpr RangeCmpStruct() = default; + RangeCmp() = default; }; //----------------------------------------------------------------------- -//! \struct RangeCmp2SatelliteBlockStruct +//! \struct RangeCmp2SatelliteBlock //! \brief Compressed satellite block from a OEM4 binary RANGECMP2 //! message. //----------------------------------------------------------------------- -struct RangeCmp2SatelliteBlockStruct +struct RangeCmp2SatelliteBlock { uint8_t ucSVChanNumber{0}; uint8_t ucSatelliteIdentifier{0}; uint64_t ulCombinedField{0}; - constexpr RangeCmp2SatelliteBlockStruct() = default; + RangeCmp2SatelliteBlock() = default; }; //----------------------------------------------------------------------- -//! \struct RangeCmp2SignalBlockStruct +//! \struct RangeCmp2SignalBlock //! \brief Compressed signal block from a OEM4 binary RANGECMP2 message. //----------------------------------------------------------------------- -struct RangeCmp2SignalBlockStruct +struct RangeCmp2SignalBlock { uint32_t uiCombinedField1{0}; - uint64_t ulCombinedField2{0}; + uint64_t ullCombinedField2{0}; - constexpr RangeCmp2SignalBlockStruct() = default; + RangeCmp2SignalBlock() = default; }; //----------------------------------------------------------------------- -//! \struct RangeCmp2LockTimeInfoStruct +//! \struct RangeCmp2LockTimeInfo //! \brief Store persistent data for RANGECMP2 lock time extension. -struct RangeCmp2LockTimeInfoStruct +struct RangeCmp2LockTimeInfo { double dLockTimeSaturatedMilliseconds{0.0}; // The time (milliseconds from OEM header) at which the locktime became saturated. bool bLockTimeSaturated{false}; // A flag to verify if dLockTimeSaturatedMilliseconds has been set. - constexpr RangeCmp2LockTimeInfoStruct() = default; + RangeCmp2LockTimeInfo() = default; }; -namespace RangeCmp2 { +namespace rangecmp2 { //-------------------------------------------------------------------- //! \enum SIGNAL_TYPE //! \brief RANGECMP2 message Signal Type value bitfield @@ -498,18 +495,18 @@ enum class SIGNAL_TYPE // NAVIC NAVIC_L5SPS = 1 }; -} // namespace RangeCmp2 +} // namespace rangecmp2 //----------------------------------------------------------------------- -//! \struct RangeCmp2Struct +//! \struct RangeCmp2 //! \brief OEM4 binary RANGECMP2 message representation. //----------------------------------------------------------------------- -struct RangeCmp2Struct +struct RangeCmp2 { uint32_t uiNumberOfRangeDataBytes{0}; - uint8_t aucRangeData[(RANGE_RECORD_MAX * (sizeof(RangeCmp2SatelliteBlockStruct) + sizeof(RangeCmp2SignalBlockStruct)))]{0}; + uint8_t aucRangeData[(RANGE_RECORD_MAX * (sizeof(RangeCmp2SatelliteBlock) + sizeof(RangeCmp2SignalBlock)))]{0}; - constexpr RangeCmp2Struct() = default; + RangeCmp2() = default; }; #pragma pack(pop) @@ -521,25 +518,25 @@ struct RangeCmp2Struct //! following structures. These are for containment purposes only. //----------------------------------------------------------------------- -//! \struct RangeCmp4MeasurementBlockHeaderStruct +//! \struct RangeCmp4MeasurementBlockHeader //! \brief Measurement Block Header structure to contain the values //! within the compressed bitfields for OEM4 RANGECMP4 messages. //----------------------------------------------------------------------- -struct RangeCmp4MeasurementBlockHeaderStruct +struct RangeCmp4MeasurementBlockHeader { bool bIsDifferentialData{false}; uint8_t ucReferenceDataBlockID{0}; int8_t cGLONASSFrequencyNumber{0}; - constexpr RangeCmp4MeasurementBlockHeaderStruct() = default; + RangeCmp4MeasurementBlockHeader() = default; }; //----------------------------------------------------------------------- -//! \struct RangeCmp4MeasurementSignalBlockStruct +//! \struct RangeCmp4MeasurementSignalBlock //! \brief Measurement Signal Block structure to contain the //! values within the compressed bitfields for OEM4 RANGECMP4 messages. //----------------------------------------------------------------------- -struct RangeCmp4MeasurementSignalBlockStruct +struct RangeCmp4MeasurementSignalBlock { bool bParityKnown{false}; bool bHalfCycleAdded{false}; @@ -554,29 +551,24 @@ struct RangeCmp4MeasurementSignalBlockStruct double dDoppler{0.0}; bool bValidDoppler{false}; - constexpr RangeCmp4MeasurementSignalBlockStruct() = default; + RangeCmp4MeasurementSignalBlock() = default; }; //----------------------------------------------------------------------- -//! \struct RangeCmp4LocktimeInfoStruct +//! \struct RangeCmp4LocktimeInfo //! \brief Store persistent data for RANGECMP4 locktime extrapolation. //----------------------------------------------------------------------- -struct RangeCmp4LocktimeInfoStruct +struct RangeCmp4LocktimeInfo { double dLocktimeMilliseconds{0.0}; // The current running locktime for this observation. double dLastBitfieldChangeMilliseconds{0.0}; // The last time (milliseconds from OEM header) locktime was updated. uint8_t ucLocktimeBits{UINT8_MAX}; // The last recorded bit pattern. bool bLocktimeAbsolute{false}; // Is the lock time absolute or relative? - RangeCmp4LocktimeInfoStruct() = default; + RangeCmp4LocktimeInfo() = default; }; -//----------------------------------------------------------------------- -//! \namespace RangeCmp4 -//! \brief Containment for RANGECMP4-specific field values found in other -//! RANGE data representations. -//----------------------------------------------------------------------- -namespace RangeCmp4 { +namespace rangecmp4 { //-------------------------------------------------------------------- //! \enum SIGNAL_TYPE //! \brief RANGECMP4 message Signal Type value bitfield @@ -629,10 +621,10 @@ enum class SIGNAL_TYPE UNKNOWN = -1 }; -} // namespace RangeCmp4 +} // namespace rangecmp4 //----------------------------------------------------------------------- -//! \struct ChannelTrackingStatusStruct +//! \struct ChannelTrackingStatus //! \brief Channel Tracking Status word fields decoded. Fields are from //! https://docs.novatel.com/OEM7/Content/Logs/RANGE.htm#TrackingState. //! Not every RANGECMP* message contains a raw form of the channel @@ -642,7 +634,7 @@ enum class SIGNAL_TYPE //! tracking status word that cannot be inferred based on the data in the //! RANGECMP* log, and certain defaults must be applied. //----------------------------------------------------------------------- -struct ChannelTrackingStatusStruct +struct ChannelTrackingStatus { //----------------------------------------------------------------------- //! \enum TRACKING_STATE @@ -760,10 +752,10 @@ struct ChannelTrackingStatusStruct bool bChannelAssignmentForced{false}; //! Default constructor. - ChannelTrackingStatusStruct() = default; + ChannelTrackingStatus() = default; //! Constructor from a channel tracking status word. - ChannelTrackingStatusStruct(uint32_t uiChannelTrackingStatus_) + ChannelTrackingStatus(uint32_t uiChannelTrackingStatus_) { eTrackingState = static_cast((uiChannelTrackingStatus_ & CTS_TRACKING_STATE_MASK) >> CTS_TRACKING_STATE_SHIFT); uiSVChannelNumber = static_cast((uiChannelTrackingStatus_ & CTS_SV_CHANNEL_NUMBER_MASK) >> CTS_SV_CHANNEL_NUMBER_SHIFT); @@ -784,7 +776,7 @@ struct ChannelTrackingStatusStruct //! Constructor from the available data from a RANGECMP2 SAT/SIG block pair. //! NOTE: Some defaults exist here. - ChannelTrackingStatusStruct(const RangeCmp2SatelliteBlockStruct& stRangeCmp2SatBlock_, const RangeCmp2SignalBlockStruct& stRangeCmp2SigBlock_) + ChannelTrackingStatus(const RangeCmp2SatelliteBlock& stRangeCmp2SatBlock_, const RangeCmp2SignalBlock& stRangeCmp2SigBlock_) { bGrouped = (static_cast(stRangeCmp2SatBlock_.ulCombinedField & RC2_SAT_NUM_SIGNAL_BLOCKS_BASE_MASK) >> RC2_SAT_NUM_SIGNAL_BLOCKS_BASE_SHIFT) > 1; @@ -804,13 +796,12 @@ struct ChannelTrackingStatusStruct eSatelliteSystem = SystemToSatelliteSystem( static_cast((stRangeCmp2SatBlock_.ulCombinedField & RC2_SAT_SATELLITE_SYSTEM_ID_MASK) >> RC2_SAT_SATELLITE_SYSTEM_ID_SHIFT)); eSignalType = RangeCmp2SignalTypeToSignalType( - eSatelliteSystem, static_cast(stRangeCmp2SigBlock_.uiCombinedField1 & RC2_SIG_SIGNAL_TYPE_MASK)); + eSatelliteSystem, static_cast(stRangeCmp2SigBlock_.uiCombinedField1 & RC2_SIG_SIGNAL_TYPE_MASK)); } //! Constructor from the available data from a RANGECMP4 Primary Block and Measurement Block //! pair. - ChannelTrackingStatusStruct(SYSTEM eSystem_, RangeCmp4::SIGNAL_TYPE eSignalType_, - const RangeCmp4MeasurementSignalBlockStruct& stMeasurementBlock_) + ChannelTrackingStatus(SYSTEM eSystem_, rangecmp4::SIGNAL_TYPE eSignalType_, const RangeCmp4MeasurementSignalBlock& stMeasurementBlock_) { // Defaults that cannot be determined: eCorrelatorType = CORRELATOR_TYPE::NONE; @@ -828,10 +819,10 @@ struct ChannelTrackingStatusStruct bCodeLocked = stMeasurementBlock_.bValidPSR; bPhaseLocked = stMeasurementBlock_.bValidPhaseRange; - if (eSignalType_ == RangeCmp4::SIGNAL_TYPE::GPS_L1CA || eSignalType_ == RangeCmp4::SIGNAL_TYPE::GLONASS_L1CA || - eSignalType_ == RangeCmp4::SIGNAL_TYPE::SBAS_L1CA || eSignalType_ == RangeCmp4::SIGNAL_TYPE::GALILEO_E1 || - eSignalType_ == RangeCmp4::SIGNAL_TYPE::BEIDOU_B1I || eSignalType_ == RangeCmp4::SIGNAL_TYPE::QZSS_L1CA || - (eSatelliteSystem == SATELLITE_SYSTEM::BEIDOU && eSignalType_ == RangeCmp4::SIGNAL_TYPE::BEIDOU_B1GEO)) + if (eSignalType_ == rangecmp4::SIGNAL_TYPE::GPS_L1CA || eSignalType_ == rangecmp4::SIGNAL_TYPE::GLONASS_L1CA || + eSignalType_ == rangecmp4::SIGNAL_TYPE::SBAS_L1CA || eSignalType_ == rangecmp4::SIGNAL_TYPE::GALILEO_E1 || + eSignalType_ == rangecmp4::SIGNAL_TYPE::BEIDOU_B1I || eSignalType_ == rangecmp4::SIGNAL_TYPE::QZSS_L1CA || + (eSatelliteSystem == SATELLITE_SYSTEM::BEIDOU && eSignalType_ == rangecmp4::SIGNAL_TYPE::BEIDOU_B1GEO)) { bPrimaryL1Channel = true; eTrackingState = TRACKING_STATE::PHASE_LOCK_LOOP; @@ -844,111 +835,111 @@ struct ChannelTrackingStatusStruct } //! Convert a RANGECMP2 signal type to the channel tracking status enumeration. - static SIGNAL_TYPE RangeCmp2SignalTypeToSignalType(SATELLITE_SYSTEM eSystem_, RangeCmp2::SIGNAL_TYPE eSignalType_) + static SIGNAL_TYPE RangeCmp2SignalTypeToSignalType(SATELLITE_SYSTEM eSystem_, rangecmp2::SIGNAL_TYPE eSignalType_) { switch (eSystem_) { case SATELLITE_SYSTEM::GPS: - return eSignalType_ == RangeCmp2::SIGNAL_TYPE::GPS_L1CA ? SIGNAL_TYPE::GPS_L1CA - : eSignalType_ == RangeCmp2::SIGNAL_TYPE::GPS_L2Y ? SIGNAL_TYPE::GPS_L2Y - : eSignalType_ == RangeCmp2::SIGNAL_TYPE::GPS_L2CM ? SIGNAL_TYPE::GPS_L2CM - : eSignalType_ == RangeCmp2::SIGNAL_TYPE::GPS_L2P ? SIGNAL_TYPE::GPS_L2P - : eSignalType_ == RangeCmp2::SIGNAL_TYPE::GPS_L5Q ? SIGNAL_TYPE::GPS_L5Q - : eSignalType_ == RangeCmp2::SIGNAL_TYPE::GPS_L1C ? SIGNAL_TYPE::GPS_L1CP + return eSignalType_ == rangecmp2::SIGNAL_TYPE::GPS_L1CA ? SIGNAL_TYPE::GPS_L1CA + : eSignalType_ == rangecmp2::SIGNAL_TYPE::GPS_L2Y ? SIGNAL_TYPE::GPS_L2Y + : eSignalType_ == rangecmp2::SIGNAL_TYPE::GPS_L2CM ? SIGNAL_TYPE::GPS_L2CM + : eSignalType_ == rangecmp2::SIGNAL_TYPE::GPS_L2P ? SIGNAL_TYPE::GPS_L2P + : eSignalType_ == rangecmp2::SIGNAL_TYPE::GPS_L5Q ? SIGNAL_TYPE::GPS_L5Q + : eSignalType_ == rangecmp2::SIGNAL_TYPE::GPS_L1C ? SIGNAL_TYPE::GPS_L1CP : SIGNAL_TYPE::UNKNOWN; case SATELLITE_SYSTEM::GLONASS: - return eSignalType_ == RangeCmp2::SIGNAL_TYPE::GLONASS_L1CA ? SIGNAL_TYPE::GLONASS_L1CA - : eSignalType_ == RangeCmp2::SIGNAL_TYPE::GLONASS_L2CA ? SIGNAL_TYPE::GLONASS_L2CA - : eSignalType_ == RangeCmp2::SIGNAL_TYPE::GLONASS_L2P ? SIGNAL_TYPE::GLONASS_L2P - : eSignalType_ == RangeCmp2::SIGNAL_TYPE::GLONASS_L3Q ? SIGNAL_TYPE::GLONASS_L3Q + return eSignalType_ == rangecmp2::SIGNAL_TYPE::GLONASS_L1CA ? SIGNAL_TYPE::GLONASS_L1CA + : eSignalType_ == rangecmp2::SIGNAL_TYPE::GLONASS_L2CA ? SIGNAL_TYPE::GLONASS_L2CA + : eSignalType_ == rangecmp2::SIGNAL_TYPE::GLONASS_L2P ? SIGNAL_TYPE::GLONASS_L2P + : eSignalType_ == rangecmp2::SIGNAL_TYPE::GLONASS_L3Q ? SIGNAL_TYPE::GLONASS_L3Q : SIGNAL_TYPE::UNKNOWN; case SATELLITE_SYSTEM::BEIDOU: - return eSignalType_ == RangeCmp2::SIGNAL_TYPE::BEIDOU_B1D1I ? SIGNAL_TYPE::BEIDOU_B1ID1 - : eSignalType_ == RangeCmp2::SIGNAL_TYPE::BEIDOU_B1D2I ? SIGNAL_TYPE::BEIDOU_B1ID2 - : eSignalType_ == RangeCmp2::SIGNAL_TYPE::BEIDOU_B2D1I ? SIGNAL_TYPE::BEIDOU_B2ID1 - : eSignalType_ == RangeCmp2::SIGNAL_TYPE::BEIDOU_B2D2I ? SIGNAL_TYPE::BEIDOU_B2ID2 - : eSignalType_ == RangeCmp2::SIGNAL_TYPE::BEIDOU_B3D1I ? SIGNAL_TYPE::BEIDOU_B3ID1 - : eSignalType_ == RangeCmp2::SIGNAL_TYPE::BEIDOU_B3D2I ? SIGNAL_TYPE::BEIDOU_B3ID2 - : eSignalType_ == RangeCmp2::SIGNAL_TYPE::BEIDOU_B1CP ? SIGNAL_TYPE::BEIDOU_B1CP - : eSignalType_ == RangeCmp2::SIGNAL_TYPE::BEIDOU_B2AP ? SIGNAL_TYPE::BEIDOU_B2AP - : eSignalType_ == RangeCmp2::SIGNAL_TYPE::BEIDOU_B2B_I ? SIGNAL_TYPE::BEIDOU_B2BI + return eSignalType_ == rangecmp2::SIGNAL_TYPE::BEIDOU_B1D1I ? SIGNAL_TYPE::BEIDOU_B1ID1 + : eSignalType_ == rangecmp2::SIGNAL_TYPE::BEIDOU_B1D2I ? SIGNAL_TYPE::BEIDOU_B1ID2 + : eSignalType_ == rangecmp2::SIGNAL_TYPE::BEIDOU_B2D1I ? SIGNAL_TYPE::BEIDOU_B2ID1 + : eSignalType_ == rangecmp2::SIGNAL_TYPE::BEIDOU_B2D2I ? SIGNAL_TYPE::BEIDOU_B2ID2 + : eSignalType_ == rangecmp2::SIGNAL_TYPE::BEIDOU_B3D1I ? SIGNAL_TYPE::BEIDOU_B3ID1 + : eSignalType_ == rangecmp2::SIGNAL_TYPE::BEIDOU_B3D2I ? SIGNAL_TYPE::BEIDOU_B3ID2 + : eSignalType_ == rangecmp2::SIGNAL_TYPE::BEIDOU_B1CP ? SIGNAL_TYPE::BEIDOU_B1CP + : eSignalType_ == rangecmp2::SIGNAL_TYPE::BEIDOU_B2AP ? SIGNAL_TYPE::BEIDOU_B2AP + : eSignalType_ == rangecmp2::SIGNAL_TYPE::BEIDOU_B2B_I ? SIGNAL_TYPE::BEIDOU_B2BI : SIGNAL_TYPE::UNKNOWN; case SATELLITE_SYSTEM::GALILEO: - return eSignalType_ == RangeCmp2::SIGNAL_TYPE::GALILEO_E1C ? SIGNAL_TYPE::GALILEO_E1C - : eSignalType_ == RangeCmp2::SIGNAL_TYPE::GALILEO_E5AQ ? SIGNAL_TYPE::GALILEO_E5AQ - : eSignalType_ == RangeCmp2::SIGNAL_TYPE::GALILEO_E5BQ ? SIGNAL_TYPE::GALILEO_E5BQ - : eSignalType_ == RangeCmp2::SIGNAL_TYPE::GALILEO_ALTBOCQ ? SIGNAL_TYPE::GALILEO_E5ALTBOCQ - : eSignalType_ == RangeCmp2::SIGNAL_TYPE::GALILEO_E6C ? SIGNAL_TYPE::GALILEO_E6C - : eSignalType_ == RangeCmp2::SIGNAL_TYPE::GALILEO_E6B ? SIGNAL_TYPE::GALILEO_E6B + return eSignalType_ == rangecmp2::SIGNAL_TYPE::GALILEO_E1C ? SIGNAL_TYPE::GALILEO_E1C + : eSignalType_ == rangecmp2::SIGNAL_TYPE::GALILEO_E5AQ ? SIGNAL_TYPE::GALILEO_E5AQ + : eSignalType_ == rangecmp2::SIGNAL_TYPE::GALILEO_E5BQ ? SIGNAL_TYPE::GALILEO_E5BQ + : eSignalType_ == rangecmp2::SIGNAL_TYPE::GALILEO_ALTBOCQ ? SIGNAL_TYPE::GALILEO_E5ALTBOCQ + : eSignalType_ == rangecmp2::SIGNAL_TYPE::GALILEO_E6C ? SIGNAL_TYPE::GALILEO_E6C + : eSignalType_ == rangecmp2::SIGNAL_TYPE::GALILEO_E6B ? SIGNAL_TYPE::GALILEO_E6B : SIGNAL_TYPE::UNKNOWN; case SATELLITE_SYSTEM::QZSS: - return eSignalType_ == RangeCmp2::SIGNAL_TYPE::QZSS_L1CA ? SIGNAL_TYPE::QZSS_L1CA - : eSignalType_ == RangeCmp2::SIGNAL_TYPE::QZSS_L2CM ? SIGNAL_TYPE::QZSS_L2CM - : eSignalType_ == RangeCmp2::SIGNAL_TYPE::QZSS_L5Q ? SIGNAL_TYPE::QZSS_L5Q - : eSignalType_ == RangeCmp2::SIGNAL_TYPE::QZSS_L1C ? SIGNAL_TYPE::QZSS_L1CP - : eSignalType_ == RangeCmp2::SIGNAL_TYPE::QZSS_L6P ? SIGNAL_TYPE::QZSS_L6P + return eSignalType_ == rangecmp2::SIGNAL_TYPE::QZSS_L1CA ? SIGNAL_TYPE::QZSS_L1CA + : eSignalType_ == rangecmp2::SIGNAL_TYPE::QZSS_L2CM ? SIGNAL_TYPE::QZSS_L2CM + : eSignalType_ == rangecmp2::SIGNAL_TYPE::QZSS_L5Q ? SIGNAL_TYPE::QZSS_L5Q + : eSignalType_ == rangecmp2::SIGNAL_TYPE::QZSS_L1C ? SIGNAL_TYPE::QZSS_L1CP + : eSignalType_ == rangecmp2::SIGNAL_TYPE::QZSS_L6P ? SIGNAL_TYPE::QZSS_L6P : SIGNAL_TYPE::UNKNOWN; case SATELLITE_SYSTEM::SBAS: - return eSignalType_ == RangeCmp2::SIGNAL_TYPE::SBAS_L1CA ? SIGNAL_TYPE::SBAS_L1CA - : eSignalType_ == RangeCmp2::SIGNAL_TYPE::SBAS_L5I ? SIGNAL_TYPE::SBAS_L5I + return eSignalType_ == rangecmp2::SIGNAL_TYPE::SBAS_L1CA ? SIGNAL_TYPE::SBAS_L1CA + : eSignalType_ == rangecmp2::SIGNAL_TYPE::SBAS_L5I ? SIGNAL_TYPE::SBAS_L5I : SIGNAL_TYPE::UNKNOWN; - case SATELLITE_SYSTEM::NAVIC: return eSignalType_ == RangeCmp2::SIGNAL_TYPE::NAVIC_L5SPS ? SIGNAL_TYPE::NAVIC_L5SPS : SIGNAL_TYPE::UNKNOWN; - case SATELLITE_SYSTEM::OTHER: return eSignalType_ == RangeCmp2::SIGNAL_TYPE::LBAND ? SIGNAL_TYPE::LBAND : SIGNAL_TYPE::UNKNOWN; + case SATELLITE_SYSTEM::NAVIC: return eSignalType_ == rangecmp2::SIGNAL_TYPE::NAVIC_L5SPS ? SIGNAL_TYPE::NAVIC_L5SPS : SIGNAL_TYPE::UNKNOWN; + case SATELLITE_SYSTEM::OTHER: return eSignalType_ == rangecmp2::SIGNAL_TYPE::LBAND ? SIGNAL_TYPE::LBAND : SIGNAL_TYPE::UNKNOWN; default: return SIGNAL_TYPE::UNKNOWN; } } //! Convert a RANGECMP4 signal type to the channel tracking status enumeration. - static SIGNAL_TYPE RangeCmp4SignalTypeToSignalType(SATELLITE_SYSTEM eSystem_, RangeCmp4::SIGNAL_TYPE eSignalType_) + static SIGNAL_TYPE RangeCmp4SignalTypeToSignalType(SATELLITE_SYSTEM eSystem_, rangecmp4::SIGNAL_TYPE eSignalType_) { switch (eSystem_) { case SATELLITE_SYSTEM::GPS: - return eSignalType_ == RangeCmp4::SIGNAL_TYPE::GPS_L1CA ? SIGNAL_TYPE::GPS_L1CA - : eSignalType_ == RangeCmp4::SIGNAL_TYPE::GPS_L2Y ? SIGNAL_TYPE::GPS_L2Y - : eSignalType_ == RangeCmp4::SIGNAL_TYPE::GPS_L2C ? SIGNAL_TYPE::GPS_L2CM - : eSignalType_ == RangeCmp4::SIGNAL_TYPE::GPS_L2P ? SIGNAL_TYPE::GPS_L2P - : eSignalType_ == RangeCmp4::SIGNAL_TYPE::GPS_L5Q ? SIGNAL_TYPE::GPS_L5Q - : eSignalType_ == RangeCmp4::SIGNAL_TYPE::GPS_L1C ? SIGNAL_TYPE::GPS_L1CP + return eSignalType_ == rangecmp4::SIGNAL_TYPE::GPS_L1CA ? SIGNAL_TYPE::GPS_L1CA + : eSignalType_ == rangecmp4::SIGNAL_TYPE::GPS_L2Y ? SIGNAL_TYPE::GPS_L2Y + : eSignalType_ == rangecmp4::SIGNAL_TYPE::GPS_L2C ? SIGNAL_TYPE::GPS_L2CM + : eSignalType_ == rangecmp4::SIGNAL_TYPE::GPS_L2P ? SIGNAL_TYPE::GPS_L2P + : eSignalType_ == rangecmp4::SIGNAL_TYPE::GPS_L5Q ? SIGNAL_TYPE::GPS_L5Q + : eSignalType_ == rangecmp4::SIGNAL_TYPE::GPS_L1C ? SIGNAL_TYPE::GPS_L1CP : SIGNAL_TYPE::UNKNOWN; case SATELLITE_SYSTEM::GLONASS: - return eSignalType_ == RangeCmp4::SIGNAL_TYPE::GLONASS_L1CA ? SIGNAL_TYPE::GLONASS_L1CA - : eSignalType_ == RangeCmp4::SIGNAL_TYPE::GLONASS_L2CA ? SIGNAL_TYPE::GLONASS_L2CA - : eSignalType_ == RangeCmp4::SIGNAL_TYPE::GLONASS_L2P ? SIGNAL_TYPE::GLONASS_L2P - : eSignalType_ == RangeCmp4::SIGNAL_TYPE::GLONASS_L3 ? SIGNAL_TYPE::GLONASS_L3Q + return eSignalType_ == rangecmp4::SIGNAL_TYPE::GLONASS_L1CA ? SIGNAL_TYPE::GLONASS_L1CA + : eSignalType_ == rangecmp4::SIGNAL_TYPE::GLONASS_L2CA ? SIGNAL_TYPE::GLONASS_L2CA + : eSignalType_ == rangecmp4::SIGNAL_TYPE::GLONASS_L2P ? SIGNAL_TYPE::GLONASS_L2P + : eSignalType_ == rangecmp4::SIGNAL_TYPE::GLONASS_L3 ? SIGNAL_TYPE::GLONASS_L3Q : SIGNAL_TYPE::UNKNOWN; case SATELLITE_SYSTEM::BEIDOU: - return eSignalType_ == RangeCmp4::SIGNAL_TYPE::BEIDOU_B1I ? SIGNAL_TYPE::BEIDOU_B1ID1 - : eSignalType_ == RangeCmp4::SIGNAL_TYPE::BEIDOU_B1GEO ? SIGNAL_TYPE::BEIDOU_B1ID2 - : eSignalType_ == RangeCmp4::SIGNAL_TYPE::BEIDOU_B2I ? SIGNAL_TYPE::BEIDOU_B2ID1 - : eSignalType_ == RangeCmp4::SIGNAL_TYPE::BEIDOU_B2GEO ? SIGNAL_TYPE::BEIDOU_B2ID2 - : eSignalType_ == RangeCmp4::SIGNAL_TYPE::BEIDOU_B3I ? SIGNAL_TYPE::BEIDOU_B3ID1 - : eSignalType_ == RangeCmp4::SIGNAL_TYPE::BEIDOU_B3GEO ? SIGNAL_TYPE::BEIDOU_B3ID2 - : eSignalType_ == RangeCmp4::SIGNAL_TYPE::BEIDOU_B1CP ? SIGNAL_TYPE::BEIDOU_B1CP - : eSignalType_ == RangeCmp4::SIGNAL_TYPE::BEIDOU_B2AP ? SIGNAL_TYPE::BEIDOU_B2AP - : eSignalType_ == RangeCmp4::SIGNAL_TYPE::BEIDOU_B2BI ? SIGNAL_TYPE::BEIDOU_B2BI + return eSignalType_ == rangecmp4::SIGNAL_TYPE::BEIDOU_B1I ? SIGNAL_TYPE::BEIDOU_B1ID1 + : eSignalType_ == rangecmp4::SIGNAL_TYPE::BEIDOU_B1GEO ? SIGNAL_TYPE::BEIDOU_B1ID2 + : eSignalType_ == rangecmp4::SIGNAL_TYPE::BEIDOU_B2I ? SIGNAL_TYPE::BEIDOU_B2ID1 + : eSignalType_ == rangecmp4::SIGNAL_TYPE::BEIDOU_B2GEO ? SIGNAL_TYPE::BEIDOU_B2ID2 + : eSignalType_ == rangecmp4::SIGNAL_TYPE::BEIDOU_B3I ? SIGNAL_TYPE::BEIDOU_B3ID1 + : eSignalType_ == rangecmp4::SIGNAL_TYPE::BEIDOU_B3GEO ? SIGNAL_TYPE::BEIDOU_B3ID2 + : eSignalType_ == rangecmp4::SIGNAL_TYPE::BEIDOU_B1CP ? SIGNAL_TYPE::BEIDOU_B1CP + : eSignalType_ == rangecmp4::SIGNAL_TYPE::BEIDOU_B2AP ? SIGNAL_TYPE::BEIDOU_B2AP + : eSignalType_ == rangecmp4::SIGNAL_TYPE::BEIDOU_B2BI ? SIGNAL_TYPE::BEIDOU_B2BI : SIGNAL_TYPE::UNKNOWN; case SATELLITE_SYSTEM::GALILEO: - return eSignalType_ == RangeCmp4::SIGNAL_TYPE::GALILEO_E1 ? SIGNAL_TYPE::GALILEO_E1C - : eSignalType_ == RangeCmp4::SIGNAL_TYPE::GALILEO_E5A ? SIGNAL_TYPE::GALILEO_E5AQ - : eSignalType_ == RangeCmp4::SIGNAL_TYPE::GALILEO_E5B ? SIGNAL_TYPE::GALILEO_E5BQ - : eSignalType_ == RangeCmp4::SIGNAL_TYPE::GALILEO_ALTBOC ? SIGNAL_TYPE::GALILEO_E5ALTBOCQ - : eSignalType_ == RangeCmp4::SIGNAL_TYPE::GALILEO_E6C ? SIGNAL_TYPE::GALILEO_E6C - : eSignalType_ == RangeCmp4::SIGNAL_TYPE::GALILEO_E6B ? SIGNAL_TYPE::GALILEO_E6B + return eSignalType_ == rangecmp4::SIGNAL_TYPE::GALILEO_E1 ? SIGNAL_TYPE::GALILEO_E1C + : eSignalType_ == rangecmp4::SIGNAL_TYPE::GALILEO_E5A ? SIGNAL_TYPE::GALILEO_E5AQ + : eSignalType_ == rangecmp4::SIGNAL_TYPE::GALILEO_E5B ? SIGNAL_TYPE::GALILEO_E5BQ + : eSignalType_ == rangecmp4::SIGNAL_TYPE::GALILEO_ALTBOC ? SIGNAL_TYPE::GALILEO_E5ALTBOCQ + : eSignalType_ == rangecmp4::SIGNAL_TYPE::GALILEO_E6C ? SIGNAL_TYPE::GALILEO_E6C + : eSignalType_ == rangecmp4::SIGNAL_TYPE::GALILEO_E6B ? SIGNAL_TYPE::GALILEO_E6B : SIGNAL_TYPE::UNKNOWN; case SATELLITE_SYSTEM::QZSS: - return eSignalType_ == RangeCmp4::SIGNAL_TYPE::QZSS_L1CA ? SIGNAL_TYPE::QZSS_L1CA - : eSignalType_ == RangeCmp4::SIGNAL_TYPE::QZSS_L2C ? SIGNAL_TYPE::QZSS_L2CM - : eSignalType_ == RangeCmp4::SIGNAL_TYPE::QZSS_L5Q ? SIGNAL_TYPE::QZSS_L5Q - : eSignalType_ == RangeCmp4::SIGNAL_TYPE::QZSS_L1C ? SIGNAL_TYPE::QZSS_L1CP - : eSignalType_ == RangeCmp4::SIGNAL_TYPE::QZSS_L6D ? SIGNAL_TYPE::QZSS_L6D - : eSignalType_ == RangeCmp4::SIGNAL_TYPE::QZSS_L6P ? SIGNAL_TYPE::QZSS_L6P + return eSignalType_ == rangecmp4::SIGNAL_TYPE::QZSS_L1CA ? SIGNAL_TYPE::QZSS_L1CA + : eSignalType_ == rangecmp4::SIGNAL_TYPE::QZSS_L2C ? SIGNAL_TYPE::QZSS_L2CM + : eSignalType_ == rangecmp4::SIGNAL_TYPE::QZSS_L5Q ? SIGNAL_TYPE::QZSS_L5Q + : eSignalType_ == rangecmp4::SIGNAL_TYPE::QZSS_L1C ? SIGNAL_TYPE::QZSS_L1CP + : eSignalType_ == rangecmp4::SIGNAL_TYPE::QZSS_L6D ? SIGNAL_TYPE::QZSS_L6D + : eSignalType_ == rangecmp4::SIGNAL_TYPE::QZSS_L6P ? SIGNAL_TYPE::QZSS_L6P : SIGNAL_TYPE::UNKNOWN; case SATELLITE_SYSTEM::SBAS: - return eSignalType_ == RangeCmp4::SIGNAL_TYPE::SBAS_L1CA ? SIGNAL_TYPE::SBAS_L1CA - : eSignalType_ == RangeCmp4::SIGNAL_TYPE::SBAS_L5I ? SIGNAL_TYPE::SBAS_L5I + return eSignalType_ == rangecmp4::SIGNAL_TYPE::SBAS_L1CA ? SIGNAL_TYPE::SBAS_L1CA + : eSignalType_ == rangecmp4::SIGNAL_TYPE::SBAS_L5I ? SIGNAL_TYPE::SBAS_L5I : SIGNAL_TYPE::UNKNOWN; - case SATELLITE_SYSTEM::NAVIC: return eSignalType_ == RangeCmp4::SIGNAL_TYPE::NAVIC_L5SPS ? SIGNAL_TYPE::NAVIC_L5SPS : SIGNAL_TYPE::UNKNOWN; + case SATELLITE_SYSTEM::NAVIC: return eSignalType_ == rangecmp4::SIGNAL_TYPE::NAVIC_L5SPS ? SIGNAL_TYPE::NAVIC_L5SPS : SIGNAL_TYPE::UNKNOWN; default: return SIGNAL_TYPE::UNKNOWN; } } diff --git a/include/novatel_edie/decoders/oem/rangecmp/range_decompressor.hpp b/include/novatel_edie/decoders/oem/rangecmp/range_decompressor.hpp index 7ba4a88db..0ed4350fb 100644 --- a/include/novatel_edie/decoders/oem/rangecmp/range_decompressor.hpp +++ b/include/novatel_edie/decoders/oem/rangecmp/range_decompressor.hpp @@ -51,14 +51,14 @@ class RangeDecompressor // //! \return A shared_ptr to the spdlog::logger. //---------------------------------------------------------------------------- - std::shared_ptr GetLogger(); + std::shared_ptr GetLogger() { return pclMyLogger; } //---------------------------------------------------------------------------- //! \brief Set the level of detail produced by the internal logger. // //! \param[in] eLevel_ The logging level to enable. //---------------------------------------------------------------------------- - void SetLoggerLevel(spdlog::level::level_enum eLevel_); + void SetLoggerLevel(spdlog::level::level_enum eLevel_) { pclMyLogger->set_level(eLevel_); } //---------------------------------------------------------------------------- //! \brief Reset the decompressor to handle new datasets @@ -71,9 +71,17 @@ class RangeDecompressor ammmMyRangeCmp4LockTimes[static_cast(MEASUREMENT_SOURCE::SECONDARY)].clear(); } - [[nodiscard]] STATUS Decompress(unsigned char* pucRangeMessageBuffer_, uint32_t uiRangeMessageBufferSize_, MetaDataStruct& stMetaData_, + [[nodiscard]] STATUS Decompress(unsigned char* pucBuffer_, uint32_t uiBufferSize_, MetaDataStruct& stMetaData_, ENCODE_FORMAT eFormat_ = ENCODE_FORMAT::UNSPECIFIED); + protected: + std::map>> + ammmMyRangeCmp2LockTimes[static_cast(MEASUREMENT_SOURCE::MAX)]; + std::map>> + ammmMyRangeCmp4LockTimes[static_cast(MEASUREMENT_SOURCE::MAX)]; + + template T ExtractBitfield(unsigned char** ppucData_, uint32_t& uiBytesLeft_, uint32_t& uiBitOffset_, uint32_t uiBitsInBitfield_); + private: Filter clMyRangeCmpFilter; HeaderDecoder clMyHeaderDecoder; @@ -84,54 +92,35 @@ class RangeDecompressor JsonReader* pclMyMsgDB{}; // Store the last primary reference blocks for each measurement source. - RangeCmp4MeasurementSignalBlockStruct astMyLastPrimaryReferenceBlocks[static_cast(MEASUREMENT_SOURCE::MAX)]; + RangeCmp4MeasurementSignalBlock astMyLastPrimaryReferenceBlocks[static_cast(MEASUREMENT_SOURCE::MAX)]; - // TODO: Is there a better way to map all this information together? - // This is an array of map of map of maps. Indexed by SYSTEM, RangeCmp4::SIGNAL_TYPE, then PRN + // This is an array of map of map of maps. Indexed by SYSTEM, rangecmp4::SIGNAL_TYPE, then PRN // (uint32_t). This will store a header and its reference block for whenever we find // differential data for the System, Signal type and PRN. We must keep track of which // measurement source the reference block came from so any subsequent differential blocks are // correctly decompressed. - std::map>>> + std::map>>> ammmMyReferenceBlocks[static_cast(MEASUREMENT_SOURCE::MAX)]; - // Protected members to be accessed by test child classes. - protected: - std::map>> - ammmMyRangeCmp2LockTimes[static_cast(MEASUREMENT_SOURCE::MAX)]; - std::map>> - ammmMyRangeCmp4LockTimes[static_cast(MEASUREMENT_SOURCE::MAX)]; - - private: - static double GetSignalWavelength(const ChannelTrackingStatusStruct& stChannelTrackingStatus_, int16_t sGLONASSFrequency_); - float DetermineRangeCmp2ObservationLockTime(const MetaDataStruct& stMetaData_, uint32_t uiLockTimeBits_, - ChannelTrackingStatusStruct::SATELLITE_SYSTEM eSystem_, - ChannelTrackingStatusStruct::SIGNAL_TYPE eSignal_, uint16_t usPRN_); - float DetermineRangeCmp4ObservationLockTime(const MetaDataStruct& stMetaData_, uint8_t ucLockTimeBits_, - ChannelTrackingStatusStruct::SATELLITE_SYSTEM eSystem_, - ChannelTrackingStatusStruct::SIGNAL_TYPE eSignal_, uint32_t uiPRN_); - template - void DecompressReferenceBlock(uint8_t** ppucDataPointer_, RangeCmp4MeasurementSignalBlockStruct& stReferenceBlock_, - MEASUREMENT_SOURCE eMeasurementSource_); - template - void DecompressDifferentialBlock(uint8_t** ppucDataPointer_, RangeCmp4MeasurementSignalBlockStruct& stDifferentialBlock_, - const RangeCmp4MeasurementSignalBlockStruct& stReferenceBlock_, double dSecondOffset_); - void PopulateNextRangeData(RangeDataStruct& stRangeData_, const RangeCmp4MeasurementSignalBlockStruct& stBlock_, - const MetaDataStruct& stMetaData_, const ChannelTrackingStatusStruct& stChannelTrackingStatus_, uint32_t uiPRN_, - char cGLONASSFrequencyNumber_); - - static void RangeCmpToRange(const RangeCmpStruct& stRangeCmpMessage_, RangeStruct& stRangeMessage_); - void RangeCmp2ToRange(const RangeCmp2Struct& stRangeCmp2Message_, RangeStruct& stRangeMessage_, const MetaDataStruct& stMetaData_); - void RangeCmp4ToRange(uint8_t* pucCompressedData_, RangeStruct& stRangeMessage_, const MetaDataStruct& pstMetaData_); - - // Protected members to be accessed by test child classes. - protected: - uint32_t uiMyBytesRemaining{0U}; - uint32_t uiMyBitOffset{0U}; - uint64_t GetBitfieldFromBuffer(uint8_t** ppucDataBuffer_, uint32_t uiBitsInBitfield_); + static double GetSignalWavelength(const ChannelTrackingStatus& stChannelStatus_, int16_t sGLONASSFrequency_); + float GetRangeCmp2LockTime(const MetaDataStruct& stMetaData_, uint32_t uiLockTimeBits_, ChannelTrackingStatus::SATELLITE_SYSTEM eSystem_, + ChannelTrackingStatus::SIGNAL_TYPE eSignal_, uint16_t usPRN_); + float GetRangeCmp4LockTime(const MetaDataStruct& stMetaData_, uint8_t ucLockTimeBits_, ChannelTrackingStatus::SATELLITE_SYSTEM eSystem_, + ChannelTrackingStatus::SIGNAL_TYPE eSignal_, uint32_t uiPRN_); + template + void DecompressReferenceBlock(unsigned char** ppucData_, uint32_t& uiBytesLeft_, uint32_t& uiBitOffset_, + RangeCmp4MeasurementSignalBlock& stRefBlock_, MEASUREMENT_SOURCE eMeasurementSource_); + template + void DecompressDifferentialBlock(unsigned char** ppucData_, uint32_t& uiBytesLeft_, uint32_t& uiBitOffset_, + RangeCmp4MeasurementSignalBlock& stDiffBlock_, const RangeCmp4MeasurementSignalBlock& stRefBlock_, + double dSecondOffset_); + void PopulateNextRangeData(RangeData& stRangeData_, const RangeCmp4MeasurementSignalBlock& stBlock_, const MetaDataStruct& stMetaData_, + const ChannelTrackingStatus& stChannelStatus_, uint32_t uiPRN_, char cGLONASSFrequencyNumber_); + + static void RangeCmpToRange(const RangeCmp& stRangeCmpMessage_, Range& stRangeMessage_); + void RangeCmp2ToRange(const RangeCmp2& stRangeCmp2Message_, Range& stRangeMessage_, const MetaDataStruct& stMetaData_); + void RangeCmp4ToRange(unsigned char* pucData_, Range& stRangeMessage_, const MetaDataStruct& pstMetaData_); }; } // namespace novatel::edie::oem diff --git a/src/decoders/oem/src/rangecmp/range_decompressor.cpp b/src/decoders/oem/src/rangecmp/range_decompressor.cpp index a6ec46be2..5e1683f67 100644 --- a/src/decoders/oem/src/rangecmp/range_decompressor.cpp +++ b/src/decoders/oem/src/rangecmp/range_decompressor.cpp @@ -31,164 +31,74 @@ using namespace novatel::edie; using namespace novatel::edie::oem; -//----------------------------------------------------------------------- -//! Table used to expand the scaled Pseudorange STDs. This is defined -//! in the RANGECMP documentation: -//! https://docs.novatel.com/OEM7/Content/Logs/RANGECMP.htm?Highlight=rangecmp#StdDevPSRValues -//----------------------------------------------------------------------- -constexpr float afTheRangeCmpPSRStdDevValues[] = { - // 0 1 2 3 4 5 6 7 - 0.050F, 0.075F, 0.113F, 0.169F, 0.253F, 0.380F, 0.570F, 0.854F, - // 8 9 10 11 12 13 14 15 - 1.281F, 2.375F, 4.750F, 9.500F, 19.000F, 38.000F, 76.000F, 152.000F}; - -//----------------------------------------------------------------------- -//! Table used to expand the scaled Pseudorange STDs. This is defined -//! in the RANGECMP2 documentation: -//! https://docs.novatel.com/OEM7/Content/Logs/RANGECMP2.htm?Highlight=RANGECMP2#StdDevPSRScaling -//----------------------------------------------------------------------- -constexpr float afTheRangeCmp2PSRStdDevValues[] = { - // 0 1 2 3 4 5 6 7 - 0.020F, 0.030F, 0.045F, 0.066F, 0.099F, 0.148F, 0.220F, 0.329F, - // 8 9 10 11 12 13 14 15 (>5.409) - 0.491F, 0.732F, 1.092F, 1.629F, 2.430F, 3.625F, 5.409F, 5.409F}; - -//----------------------------------------------------------------------- -//! Table used to expand the scaled Accumulated Doppler Range STDs. This -//! is defined in the RANGECMP2 documentation: -//! https://docs.novatel.com/OEM7/Content/Logs/RANGECMP2.htm?Highlight=RANGECMP2#StdDevADRScaling -//----------------------------------------------------------------------- -constexpr float afTheRangeCmp2ADRStdDevValues[] = { - // 0 1 2 3 4 5 6 7 - 0.00391F, 0.00521F, 0.00696F, 0.00929F, 0.01239F, 0.01654F, 0.02208F, 0.02947F, - // 8 9 10 11 12 13 14 15 (>0.22230) - 0.03933F, 0.05249F, 0.07006F, 0.09350F, 0.12480F, 0.16656F, 0.22230F, 0.22230F}; - //----------------------------------------------------------------------- //! Two-dimensional map to look up L1/E1/B1 Scaling for RANGECMP2 signals //! defined in the RANGECMP2 documentation: //! https://docs.novatel.com/OEM7/Content/Logs/RANGECMP2.htm?Highlight=RANGECMP2#L1_E1_B1_Scaling //----------------------------------------------------------------------- - -static std::map> mmTheRangeCmp2SignalScalingMapping = { +static std::map> mmTheRangeCmp2SignalScalingMapping = { {SYSTEM::GPS, - {{RangeCmp2::SIGNAL_TYPE::GPS_L1C, (1.0)}, - {RangeCmp2::SIGNAL_TYPE::GPS_L1CA, (1.0)}, - {RangeCmp2::SIGNAL_TYPE::GPS_L2Y, (154.0 / 120.0)}, - {RangeCmp2::SIGNAL_TYPE::GPS_L2CM, (154.0 / 120.0)}, - {RangeCmp2::SIGNAL_TYPE::GPS_L5Q, (154.0 / 115.0)}}}, + {{rangecmp2::SIGNAL_TYPE::GPS_L1C, (1.0)}, + {rangecmp2::SIGNAL_TYPE::GPS_L1CA, (1.0)}, + {rangecmp2::SIGNAL_TYPE::GPS_L2Y, (154.0 / 120.0)}, + {rangecmp2::SIGNAL_TYPE::GPS_L2CM, (154.0 / 120.0)}, + {rangecmp2::SIGNAL_TYPE::GPS_L5Q, (154.0 / 115.0)}}}, {SYSTEM::GLONASS, - {{RangeCmp2::SIGNAL_TYPE::GLONASS_L1CA, (1.0)}, - {RangeCmp2::SIGNAL_TYPE::GLONASS_L2CA, (9.0 / 7.0)}, - {RangeCmp2::SIGNAL_TYPE::GLONASS_L2P, (9.0 / 7.0)}, - {RangeCmp2::SIGNAL_TYPE::GLONASS_L3Q, (313.0 / 235.0)}}}, - {SYSTEM::SBAS, {{RangeCmp2::SIGNAL_TYPE::SBAS_L1CA, (1.0)}, {RangeCmp2::SIGNAL_TYPE::SBAS_L5I, (154.0 / 115.0)}}}, + {{rangecmp2::SIGNAL_TYPE::GLONASS_L1CA, (1.0)}, + {rangecmp2::SIGNAL_TYPE::GLONASS_L2CA, (9.0 / 7.0)}, + {rangecmp2::SIGNAL_TYPE::GLONASS_L2P, (9.0 / 7.0)}, + {rangecmp2::SIGNAL_TYPE::GLONASS_L3Q, (313.0 / 235.0)}}}, + {SYSTEM::SBAS, {{rangecmp2::SIGNAL_TYPE::SBAS_L1CA, (1.0)}, {rangecmp2::SIGNAL_TYPE::SBAS_L5I, (154.0 / 115.0)}}}, {SYSTEM::GALILEO, - {{RangeCmp2::SIGNAL_TYPE::GALILEO_E1C, (1.0)}, - {RangeCmp2::SIGNAL_TYPE::GALILEO_E5AQ, (154.0 / 115.0)}, - {RangeCmp2::SIGNAL_TYPE::GALILEO_E5BQ, (154.0 / 118.0)}, - {RangeCmp2::SIGNAL_TYPE::GALILEO_ALTBOCQ, (154.0 / 116.5)}, - {RangeCmp2::SIGNAL_TYPE::GALILEO_E6C, (154.0 / 125.0)}, - {RangeCmp2::SIGNAL_TYPE::GALILEO_E6B, (154.0 / 125.0)}}}, + {{rangecmp2::SIGNAL_TYPE::GALILEO_E1C, (1.0)}, + {rangecmp2::SIGNAL_TYPE::GALILEO_E5AQ, (154.0 / 115.0)}, + {rangecmp2::SIGNAL_TYPE::GALILEO_E5BQ, (154.0 / 118.0)}, + {rangecmp2::SIGNAL_TYPE::GALILEO_ALTBOCQ, (154.0 / 116.5)}, + {rangecmp2::SIGNAL_TYPE::GALILEO_E6C, (154.0 / 125.0)}, + {rangecmp2::SIGNAL_TYPE::GALILEO_E6B, (154.0 / 125.0)}}}, {SYSTEM::BEIDOU, - {{RangeCmp2::SIGNAL_TYPE::BEIDOU_B1D1I, (1.0)}, - {RangeCmp2::SIGNAL_TYPE::BEIDOU_B1D2I, (1.0)}, - {RangeCmp2::SIGNAL_TYPE::BEIDOU_B1CP, (1526.0 / 1540.0)}, - {RangeCmp2::SIGNAL_TYPE::BEIDOU_B2D1I, (1526.0 / 1180.0)}, - {RangeCmp2::SIGNAL_TYPE::BEIDOU_B2D2I, (1526.0 / 1180.0)}, - {RangeCmp2::SIGNAL_TYPE::BEIDOU_B2AP, (1526.0 / 1150.0)}, - {RangeCmp2::SIGNAL_TYPE::BEIDOU_B2B_I, (1526.0 / 1180.0)}, - {RangeCmp2::SIGNAL_TYPE::BEIDOU_B3D1I, (1526.0 / 1240.0)}, - {RangeCmp2::SIGNAL_TYPE::BEIDOU_B3D2I, (1526.0 / 1240.0)}}}, + {{rangecmp2::SIGNAL_TYPE::BEIDOU_B1D1I, (1.0)}, + {rangecmp2::SIGNAL_TYPE::BEIDOU_B1D2I, (1.0)}, + {rangecmp2::SIGNAL_TYPE::BEIDOU_B1CP, (1526.0 / 1540.0)}, + {rangecmp2::SIGNAL_TYPE::BEIDOU_B2D1I, (1526.0 / 1180.0)}, + {rangecmp2::SIGNAL_TYPE::BEIDOU_B2D2I, (1526.0 / 1180.0)}, + {rangecmp2::SIGNAL_TYPE::BEIDOU_B2AP, (1526.0 / 1150.0)}, + {rangecmp2::SIGNAL_TYPE::BEIDOU_B2B_I, (1526.0 / 1180.0)}, + {rangecmp2::SIGNAL_TYPE::BEIDOU_B3D1I, (1526.0 / 1240.0)}, + {rangecmp2::SIGNAL_TYPE::BEIDOU_B3D2I, (1526.0 / 1240.0)}}}, {SYSTEM::QZSS, - {{RangeCmp2::SIGNAL_TYPE::QZSS_L1C, (1.0)}, - {RangeCmp2::SIGNAL_TYPE::QZSS_L1CA, (1.0)}, - {RangeCmp2::SIGNAL_TYPE::QZSS_L2CM, (154.0 / 120.0)}, - {RangeCmp2::SIGNAL_TYPE::QZSS_L5Q, (154.0 / 115.0)}, - {RangeCmp2::SIGNAL_TYPE::QZSS_L6P, (154.0 / 125.0)}}}, - {SYSTEM::LBAND, {{RangeCmp2::SIGNAL_TYPE::LBAND, (1.0)}}}, - {SYSTEM::NAVIC, {{RangeCmp2::SIGNAL_TYPE::NAVIC_L5SPS, (1.0)}}}}; - -//----------------------------------------------------------------------- -//! A list of bitmasks to iterate easily through a RANGECMP4 message, the -//! masks are defined in the RANGECMP4 documentation: -//! https://docs.novatel.com/OEM7/Content/Logs/RANGECMP4.htm?Highlight=RANGECMP#Header -//----------------------------------------------------------------------- -constexpr SYSTEM aeTheRangeCmp4SatelliteSystems[RC4_HEADER_BLOCK_SYSTEM_COUNT]{ - SYSTEM::GPS, // bit 0 - SYSTEM::GLONASS, // bit 1 - SYSTEM::SBAS, // bit 2 - SYSTEM::GALILEO, // bit 5 - SYSTEM::BEIDOU, // bit 6 - SYSTEM::QZSS, // bit 7 - SYSTEM::NAVIC // bit 9 -}; + {{rangecmp2::SIGNAL_TYPE::QZSS_L1C, (1.0)}, + {rangecmp2::SIGNAL_TYPE::QZSS_L1CA, (1.0)}, + {rangecmp2::SIGNAL_TYPE::QZSS_L2CM, (154.0 / 120.0)}, + {rangecmp2::SIGNAL_TYPE::QZSS_L5Q, (154.0 / 115.0)}, + {rangecmp2::SIGNAL_TYPE::QZSS_L6P, (154.0 / 125.0)}}}, + {SYSTEM::LBAND, {{rangecmp2::SIGNAL_TYPE::LBAND, (1.0)}}}, + {SYSTEM::NAVIC, {{rangecmp2::SIGNAL_TYPE::NAVIC_L5SPS, (1.0)}}}}; //----------------------------------------------------------------------- //! Map of lists to look up and iterate signal enumerations in RANGECMP4 //! satellite and signal blocks defined in the RANGECMP4 documentation: //! https://docs.novatel.com/OEM7/Content/Logs/RANGECMP4.htm?Highlight=RANGECMP#Signal //----------------------------------------------------------------------- -static std::map> mvTheRangeCmp4SystemSignalMasks = { +static std::map> mvTheRangeCmp4SystemSignalMasks = { {SYSTEM::GPS, - {RangeCmp4::SIGNAL_TYPE::GPS_L1CA, RangeCmp4::SIGNAL_TYPE::GPS_L2Y, RangeCmp4::SIGNAL_TYPE::GPS_L2C, RangeCmp4::SIGNAL_TYPE::GPS_L2P, - RangeCmp4::SIGNAL_TYPE::GPS_L5Q, RangeCmp4::SIGNAL_TYPE::GPS_L1C}}, + {rangecmp4::SIGNAL_TYPE::GPS_L1CA, rangecmp4::SIGNAL_TYPE::GPS_L2Y, rangecmp4::SIGNAL_TYPE::GPS_L2C, rangecmp4::SIGNAL_TYPE::GPS_L2P, + rangecmp4::SIGNAL_TYPE::GPS_L5Q, rangecmp4::SIGNAL_TYPE::GPS_L1C}}, {SYSTEM::GLONASS, - { - - RangeCmp4::SIGNAL_TYPE::GLONASS_L1CA, RangeCmp4::SIGNAL_TYPE::GLONASS_L2CA, RangeCmp4::SIGNAL_TYPE::GLONASS_L2P, - RangeCmp4::SIGNAL_TYPE::GLONASS_L3}}, - {SYSTEM::SBAS, {RangeCmp4::SIGNAL_TYPE::SBAS_L1CA, RangeCmp4::SIGNAL_TYPE::SBAS_L5I}}, + {rangecmp4::SIGNAL_TYPE::GLONASS_L1CA, rangecmp4::SIGNAL_TYPE::GLONASS_L2CA, rangecmp4::SIGNAL_TYPE::GLONASS_L2P, + rangecmp4::SIGNAL_TYPE::GLONASS_L3}}, + {SYSTEM::SBAS, {rangecmp4::SIGNAL_TYPE::SBAS_L1CA, rangecmp4::SIGNAL_TYPE::SBAS_L5I}}, {SYSTEM::GALILEO, - {RangeCmp4::SIGNAL_TYPE::GALILEO_E1, RangeCmp4::SIGNAL_TYPE::GALILEO_E5A, RangeCmp4::SIGNAL_TYPE::GALILEO_E5B, - RangeCmp4::SIGNAL_TYPE::GALILEO_ALTBOC, RangeCmp4::SIGNAL_TYPE::GALILEO_E6C, RangeCmp4::SIGNAL_TYPE::GALILEO_E6B}}, + {rangecmp4::SIGNAL_TYPE::GALILEO_E1, rangecmp4::SIGNAL_TYPE::GALILEO_E5A, rangecmp4::SIGNAL_TYPE::GALILEO_E5B, + rangecmp4::SIGNAL_TYPE::GALILEO_ALTBOC, rangecmp4::SIGNAL_TYPE::GALILEO_E6C, rangecmp4::SIGNAL_TYPE::GALILEO_E6B}}, {SYSTEM::BEIDOU, - {RangeCmp4::SIGNAL_TYPE::BEIDOU_B1I, RangeCmp4::SIGNAL_TYPE::BEIDOU_B1GEO, RangeCmp4::SIGNAL_TYPE::BEIDOU_B2I, - RangeCmp4::SIGNAL_TYPE::BEIDOU_B2GEO, RangeCmp4::SIGNAL_TYPE::BEIDOU_B3I, RangeCmp4::SIGNAL_TYPE::BEIDOU_B3GEO, - RangeCmp4::SIGNAL_TYPE::BEIDOU_B1CP, RangeCmp4::SIGNAL_TYPE::BEIDOU_B2AP, RangeCmp4::SIGNAL_TYPE::BEIDOU_B2BI}}, + {rangecmp4::SIGNAL_TYPE::BEIDOU_B1I, rangecmp4::SIGNAL_TYPE::BEIDOU_B1GEO, rangecmp4::SIGNAL_TYPE::BEIDOU_B2I, + rangecmp4::SIGNAL_TYPE::BEIDOU_B2GEO, rangecmp4::SIGNAL_TYPE::BEIDOU_B3I, rangecmp4::SIGNAL_TYPE::BEIDOU_B3GEO, + rangecmp4::SIGNAL_TYPE::BEIDOU_B1CP, rangecmp4::SIGNAL_TYPE::BEIDOU_B2AP, rangecmp4::SIGNAL_TYPE::BEIDOU_B2BI}}, {SYSTEM::QZSS, - {RangeCmp4::SIGNAL_TYPE::QZSS_L1CA, RangeCmp4::SIGNAL_TYPE::QZSS_L2C, RangeCmp4::SIGNAL_TYPE::QZSS_L5Q, RangeCmp4::SIGNAL_TYPE::QZSS_L1C, - RangeCmp4::SIGNAL_TYPE::QZSS_L6D, RangeCmp4::SIGNAL_TYPE::QZSS_L6P}}, - {SYSTEM::NAVIC, {RangeCmp4::SIGNAL_TYPE::NAVIC_L5SPS}}}; - -//----------------------------------------------------------------------- -//! List of pre-defined floats used as translations for RANGECMP4 PSR -//! standard deviation values defined in the RANGECMP4 documentation: -//! https://docs.novatel.com/OEM7/Content/Logs/RANGECMP4.htm?Highlight=Range#Pseudora -//----------------------------------------------------------------------- -constexpr float afTheRangeCmp4PSRStdDevValues[] = { - // 0 1 2 3 4 5 6 7 - 0.020F, 0.030F, 0.045F, 0.066F, 0.099F, 0.148F, 0.220F, 0.329F, - // 8 9 10 11 12 13 14 15 (>5.410) - 0.491F, 0.732F, 1.092F, 1.629F, 2.430F, 3.625F, 5.409F, 5.409F}; - -//----------------------------------------------------------------------- -//! List of pre-defined floats used as translations for RANGECMP4 ADR -//! standard deviation values defined in the RANGECMP4 documentation: -//! https://docs.novatel.com/OEM7/Content/Logs/RANGECMP4.htm?Highlight=Range#ADR -//! Note: LSD have been removed to reduce rouning errors in the range log. -//! ADR STD is only 3 decimal places and could round up causing values -//! to be greater than values in the table. -//----------------------------------------------------------------------- -constexpr float afTheRangeCmp4ADRStdDevValues[] = { - // 0 1 2 3 4 5 6 7 - 0.003F, 0.005F, 0.007F, 0.009F, 0.012F, 0.016F, 0.022F, 0.029F, - // 8 9 10 11 12 13 14 15 (>0.2223) - 0.039F, 0.052F, 0.070F, 0.093F, 0.124F, 0.166F, 0.222F, 0.222F}; - -//----------------------------------------------------------------------- -//! List of pre-defined floats used as translations for RANGECMP4 lock -//! time values defined in the RANGECMP4 documentation: -//! https://docs.novatel.com/OEM7/Content/Logs/RANGECMP4.htm?Highlight=Range#Lock -//! NOTE: These values are the lower bound of the range representations. -//! For more information on decompressing locktime bitfields, see the -//! comment block above RangeDecompressor::DetermineRangeCmp4ObservationLocktime(). -//----------------------------------------------------------------------- -constexpr float afTheRangeCmp4LockTimeValues[] = { - // 0 1 2 3 4 5 6 7 - 0.0F, 16.0F, 32.0F, 64.0F, 128.0F, 256.0F, 512.0F, 1024.0F, - // 8 9 10 11 12 13 14 15 - 2048.0F, 4096.0F, 8192.0F, 16384.0F, 32768.0F, 65536.0F, 131072.0F, 262144.0F}; + {rangecmp4::SIGNAL_TYPE::QZSS_L1CA, rangecmp4::SIGNAL_TYPE::QZSS_L2C, rangecmp4::SIGNAL_TYPE::QZSS_L5Q, rangecmp4::SIGNAL_TYPE::QZSS_L1C, + rangecmp4::SIGNAL_TYPE::QZSS_L6D, rangecmp4::SIGNAL_TYPE::QZSS_L6P}}, + {SYSTEM::NAVIC, {rangecmp4::SIGNAL_TYPE::NAVIC_L5SPS}}}; //------------------------------------------------------------------------------ RangeDecompressor::RangeDecompressor(JsonReader* pclJsonDB_) : clMyHeaderDecoder(pclJsonDB_), clMyMessageDecoder(pclJsonDB_), clMyEncoder(pclJsonDB_) @@ -219,71 +129,67 @@ void RangeDecompressor::LoadJsonDb(JsonReader* pclJsonDB_) clMyEncoder.LoadJsonDb(pclJsonDB_); } -// ------------------------------------------------------------------------------------------------------- -void RangeDecompressor::SetLoggerLevel(spdlog::level::level_enum eLevel_) { pclMyLogger->set_level(eLevel_); } - -// ------------------------------------------------------------------------------------------------------- -std::shared_ptr RangeDecompressor::GetLogger() { return pclMyLogger; } - //------------------------------------------------------------------------------ //! This function acts as a lookup for a signal wavelength based on the provided //! ChannelTrackingStatus. Uses the Satellite System and Signal fields, and in //! the case of GLONASS, it will use the provided GLONASS frequency. //------------------------------------------------------------------------------ -double RangeDecompressor::GetSignalWavelength(const ChannelTrackingStatusStruct& stChannelTrackingStatus_, int16_t sGLONASSFrequency_) +double RangeDecompressor::GetSignalWavelength(const ChannelTrackingStatus& stChannelStatus_, int16_t sGLONASSFrequency_) { - switch (stChannelTrackingStatus_.eSatelliteSystem) + using Signal = ChannelTrackingStatus::SIGNAL_TYPE; + using System = ChannelTrackingStatus::SATELLITE_SYSTEM; + + switch (stChannelStatus_.eSatelliteSystem) { - case ChannelTrackingStatusStruct::SATELLITE_SYSTEM::GPS: - return stChannelTrackingStatus_.eSignalType == ChannelTrackingStatusStruct::SIGNAL_TYPE::GPS_L1CA ? WAVELENGTH_GPS_L1 - : stChannelTrackingStatus_.eSignalType == ChannelTrackingStatusStruct::SIGNAL_TYPE::GPS_L1CP ? WAVELENGTH_GPS_L1 - : stChannelTrackingStatus_.eSignalType == ChannelTrackingStatusStruct::SIGNAL_TYPE::GPS_L2P ? WAVELENGTH_GPS_L2 - : stChannelTrackingStatus_.eSignalType == ChannelTrackingStatusStruct::SIGNAL_TYPE::GPS_L2Y ? WAVELENGTH_GPS_L2 - : stChannelTrackingStatus_.eSignalType == ChannelTrackingStatusStruct::SIGNAL_TYPE::GPS_L2CM ? WAVELENGTH_GPS_L2 - : stChannelTrackingStatus_.eSignalType == ChannelTrackingStatusStruct::SIGNAL_TYPE::GPS_L5Q ? WAVELENGTH_GPS_L5 - : 0.0; - case ChannelTrackingStatusStruct::SATELLITE_SYSTEM::GLONASS: - return stChannelTrackingStatus_.eSignalType == ChannelTrackingStatusStruct::SIGNAL_TYPE::GLONASS_L1CA + case System::GPS: + return stChannelStatus_.eSignalType == Signal::GPS_L1CA ? WAVELENGTH_GPS_L1 + : stChannelStatus_.eSignalType == Signal::GPS_L1CP ? WAVELENGTH_GPS_L1 + : stChannelStatus_.eSignalType == Signal::GPS_L2P ? WAVELENGTH_GPS_L2 + : stChannelStatus_.eSignalType == Signal::GPS_L2Y ? WAVELENGTH_GPS_L2 + : stChannelStatus_.eSignalType == Signal::GPS_L2CM ? WAVELENGTH_GPS_L2 + : stChannelStatus_.eSignalType == Signal::GPS_L5Q ? WAVELENGTH_GPS_L5 + : 0.0; + case System::GLONASS: + return stChannelStatus_.eSignalType == Signal::GLONASS_L1CA ? SPEED_OF_LIGHT / (FREQUENCY_HZ_GLO_L1 + (sGLONASSFrequency_ * GLONASS_L1_FREQUENCY_SCALE_HZ)) - : stChannelTrackingStatus_.eSignalType == ChannelTrackingStatusStruct::SIGNAL_TYPE::GLONASS_L2CA + : stChannelStatus_.eSignalType == Signal::GLONASS_L2CA ? SPEED_OF_LIGHT / (FREQUENCY_HZ_GLO_L2 + (sGLONASSFrequency_ * GLONASS_L2_FREQUENCY_SCALE_HZ)) - : stChannelTrackingStatus_.eSignalType == ChannelTrackingStatusStruct::SIGNAL_TYPE::GLONASS_L2P + : stChannelStatus_.eSignalType == Signal::GLONASS_L2P ? SPEED_OF_LIGHT / (FREQUENCY_HZ_GLO_L2 + (sGLONASSFrequency_ * GLONASS_L2_FREQUENCY_SCALE_HZ)) - : stChannelTrackingStatus_.eSignalType == ChannelTrackingStatusStruct::SIGNAL_TYPE::GLONASS_L3Q ? WAVELENGTH_GLO_L3 - : 0.0; - case ChannelTrackingStatusStruct::SATELLITE_SYSTEM::SBAS: - return stChannelTrackingStatus_.eSignalType == ChannelTrackingStatusStruct::SIGNAL_TYPE::SBAS_L1CA ? WAVELENGTH_GPS_L1 - : stChannelTrackingStatus_.eSignalType == ChannelTrackingStatusStruct::SIGNAL_TYPE::SBAS_L5I ? WAVELENGTH_GPS_L5 - : 0.0; - case ChannelTrackingStatusStruct::SATELLITE_SYSTEM::GALILEO: - return stChannelTrackingStatus_.eSignalType == ChannelTrackingStatusStruct::SIGNAL_TYPE::GALILEO_E1C ? WAVELENGTH_GAL_E1 - : stChannelTrackingStatus_.eSignalType == ChannelTrackingStatusStruct::SIGNAL_TYPE::GALILEO_E6B ? WAVELENGTH_GAL_E6 - : stChannelTrackingStatus_.eSignalType == ChannelTrackingStatusStruct::SIGNAL_TYPE::GALILEO_E6C ? WAVELENGTH_GAL_E6 - : stChannelTrackingStatus_.eSignalType == ChannelTrackingStatusStruct::SIGNAL_TYPE::GALILEO_E5AQ ? WAVELENGTH_GAL_E5AQ - : stChannelTrackingStatus_.eSignalType == ChannelTrackingStatusStruct::SIGNAL_TYPE::GALILEO_E5BQ ? WAVELENGTH_GAL_E5BQ - : stChannelTrackingStatus_.eSignalType == ChannelTrackingStatusStruct::SIGNAL_TYPE::GALILEO_E5ALTBOCQ ? WAVELENGTH_GAL_ALTBQ - : 0.0; - case ChannelTrackingStatusStruct::SATELLITE_SYSTEM::BEIDOU: - return stChannelTrackingStatus_.eSignalType == ChannelTrackingStatusStruct::SIGNAL_TYPE::BEIDOU_B1ID1 ? WAVELENGTH_BDS_B1 - : stChannelTrackingStatus_.eSignalType == ChannelTrackingStatusStruct::SIGNAL_TYPE::BEIDOU_B1ID2 ? WAVELENGTH_BDS_B1 - : stChannelTrackingStatus_.eSignalType == ChannelTrackingStatusStruct::SIGNAL_TYPE::BEIDOU_B2ID1 ? WAVELENGTH_BDS_B2 - : stChannelTrackingStatus_.eSignalType == ChannelTrackingStatusStruct::SIGNAL_TYPE::BEIDOU_B2ID2 ? WAVELENGTH_BDS_B2 - : stChannelTrackingStatus_.eSignalType == ChannelTrackingStatusStruct::SIGNAL_TYPE::BEIDOU_B3ID1 ? WAVELENGTH_BDS_B3 - : stChannelTrackingStatus_.eSignalType == ChannelTrackingStatusStruct::SIGNAL_TYPE::BEIDOU_B3ID2 ? WAVELENGTH_BDS_B3 - : stChannelTrackingStatus_.eSignalType == ChannelTrackingStatusStruct::SIGNAL_TYPE::BEIDOU_B1CP ? WAVELENGTH_BDS_B1C - : stChannelTrackingStatus_.eSignalType == ChannelTrackingStatusStruct::SIGNAL_TYPE::BEIDOU_B2AP ? WAVELENGTH_BDS_B2A - : stChannelTrackingStatus_.eSignalType == ChannelTrackingStatusStruct::SIGNAL_TYPE::BEIDOU_B2BI ? WAVELENGTH_BDS_B2B - : 0.0; - case ChannelTrackingStatusStruct::SATELLITE_SYSTEM::QZSS: - return stChannelTrackingStatus_.eSignalType == ChannelTrackingStatusStruct::SIGNAL_TYPE::QZSS_L1CA ? WAVELENGTH_QZSS_L1 - : stChannelTrackingStatus_.eSignalType == ChannelTrackingStatusStruct::SIGNAL_TYPE::QZSS_L1CP ? WAVELENGTH_QZSS_L1 - : stChannelTrackingStatus_.eSignalType == ChannelTrackingStatusStruct::SIGNAL_TYPE::QZSS_L2CM ? WAVELENGTH_QZSS_L2 - : stChannelTrackingStatus_.eSignalType == ChannelTrackingStatusStruct::SIGNAL_TYPE::QZSS_L5Q ? WAVELENGTH_QZSS_L5 - : stChannelTrackingStatus_.eSignalType == ChannelTrackingStatusStruct::SIGNAL_TYPE::QZSS_L6P ? WAVELENGTH_QZSS_L6 - : stChannelTrackingStatus_.eSignalType == ChannelTrackingStatusStruct::SIGNAL_TYPE::QZSS_L6D ? WAVELENGTH_QZSS_L6 - : 0.0; - case ChannelTrackingStatusStruct::SATELLITE_SYSTEM::NAVIC: - return stChannelTrackingStatus_.eSignalType == ChannelTrackingStatusStruct::SIGNAL_TYPE::NAVIC_L5SPS ? WAVELENGTH_NAVIC_L5 : 0.0; + : stChannelStatus_.eSignalType == Signal::GLONASS_L3Q ? WAVELENGTH_GLO_L3 + : 0.0; + case System::SBAS: + return stChannelStatus_.eSignalType == Signal::SBAS_L1CA ? WAVELENGTH_GPS_L1 + : stChannelStatus_.eSignalType == Signal::SBAS_L5I ? WAVELENGTH_GPS_L5 + : 0.0; + case System::GALILEO: + return stChannelStatus_.eSignalType == Signal::GALILEO_E1C ? WAVELENGTH_GAL_E1 + : stChannelStatus_.eSignalType == Signal::GALILEO_E6B ? WAVELENGTH_GAL_E6 + : stChannelStatus_.eSignalType == Signal::GALILEO_E6C ? WAVELENGTH_GAL_E6 + : stChannelStatus_.eSignalType == Signal::GALILEO_E5AQ ? WAVELENGTH_GAL_E5AQ + : stChannelStatus_.eSignalType == Signal::GALILEO_E5BQ ? WAVELENGTH_GAL_E5BQ + : stChannelStatus_.eSignalType == Signal::GALILEO_E5ALTBOCQ ? WAVELENGTH_GAL_ALTBQ + : 0.0; + case System::BEIDOU: + return stChannelStatus_.eSignalType == Signal::BEIDOU_B1ID1 ? WAVELENGTH_BDS_B1 + : stChannelStatus_.eSignalType == Signal::BEIDOU_B1ID2 ? WAVELENGTH_BDS_B1 + : stChannelStatus_.eSignalType == Signal::BEIDOU_B2ID1 ? WAVELENGTH_BDS_B2 + : stChannelStatus_.eSignalType == Signal::BEIDOU_B2ID2 ? WAVELENGTH_BDS_B2 + : stChannelStatus_.eSignalType == Signal::BEIDOU_B3ID1 ? WAVELENGTH_BDS_B3 + : stChannelStatus_.eSignalType == Signal::BEIDOU_B3ID2 ? WAVELENGTH_BDS_B3 + : stChannelStatus_.eSignalType == Signal::BEIDOU_B1CP ? WAVELENGTH_BDS_B1C + : stChannelStatus_.eSignalType == Signal::BEIDOU_B2AP ? WAVELENGTH_BDS_B2A + : stChannelStatus_.eSignalType == Signal::BEIDOU_B2BI ? WAVELENGTH_BDS_B2B + : 0.0; + case System::QZSS: + return stChannelStatus_.eSignalType == Signal::QZSS_L1CA ? WAVELENGTH_QZSS_L1 + : stChannelStatus_.eSignalType == Signal::QZSS_L1CP ? WAVELENGTH_QZSS_L1 + : stChannelStatus_.eSignalType == Signal::QZSS_L2CM ? WAVELENGTH_QZSS_L2 + : stChannelStatus_.eSignalType == Signal::QZSS_L5Q ? WAVELENGTH_QZSS_L5 + : stChannelStatus_.eSignalType == Signal::QZSS_L6P ? WAVELENGTH_QZSS_L6 + : stChannelStatus_.eSignalType == Signal::QZSS_L6D ? WAVELENGTH_QZSS_L6 + : 0.0; + case System::NAVIC: return stChannelStatus_.eSignalType == Signal::NAVIC_L5SPS ? WAVELENGTH_NAVIC_L5 : 0.0; default: return 0.0; } } @@ -293,51 +199,57 @@ double RangeDecompressor::GetSignalWavelength(const ChannelTrackingStatusStruct& //! from the provided buffer pointer. It will keep an internal track of the //! current bit offset within a given byte. //------------------------------------------------------------------------------ -uint64_t RangeDecompressor::GetBitfieldFromBuffer(uint8_t** ppucDataBuffer_, uint32_t uiBitsInBitfield_) +template +T RangeDecompressor::ExtractBitfield(unsigned char** ppucData_, uint32_t& uiBytesLeft_, uint32_t& uiBitOffset_, uint32_t uiBitsInBitfield_) { - // If the user is asking for too many bits, don't do anything. - if (uiBitsInBitfield_ > (sizeof(uint64_t) * BITS_PER_BYTE)) + static_assert(std::is_integral::value || std::is_floating_point::value, "ExtractBitfield only returns integral or floating point types."); + + constexpr uint32_t typeBitSize = sizeof(T) * BITS_PER_BYTE; + + if (uiBitsInBitfield_ > typeBitSize) { - pclMyLogger->critical("Too many bits requested! Requested {}, max {}.", uiBitsInBitfield_, (sizeof(uint64_t) * BITS_PER_BYTE)); - return 0; + pclMyLogger->critical("Requested {} bits exceeds the maximum size of type T ({} bits).", uiBitsInBitfield_, typeBitSize); + return T{0}; } - // If the user is asking for more bits than are available in the buffer, don't do anything. - uint32_t uiRemainderBits = uiBitsInBitfield_ % BITS_PER_BYTE; - uint32_t uiBytesRequired = uiBitsInBitfield_ / BITS_PER_BYTE + (uiRemainderBits != 0U ? 1 : 0); - if (uiBytesRequired > uiMyBytesRemaining) + uint32_t uiBytesRequired = (uiBitsInBitfield_ + BITS_PER_BYTE - 1) / BITS_PER_BYTE; + if (uiBytesRequired > uiBytesLeft_) { - pclMyLogger->critical("Not enough bytes in this buffer. Required {}, have {}.", uiBytesRequired, uiMyBytesRemaining); - return 0; + pclMyLogger->critical("Not enough bytes in this buffer. Required {}, have {}.", uiBytesRequired, uiBytesLeft_); + return T{0}; } - // If the requested number of bits does not run into the next byte, let the remaining bits count as a remaining byte. - uiMyBytesRemaining -= uiBytesRequired - (((uiRemainderBits + uiMyBitOffset) < BITS_PER_BYTE) ? 1 : 0); + // Adjust remaining bytes by subtracting required bytes, accounting for any bit offset + uiBytesLeft_ -= uiBytesRequired - ((uiBitsInBitfield_ % BITS_PER_BYTE + uiBitOffset_) < BITS_PER_BYTE); - uint64_t ulBitfield = 0; + uint64_t ullBitfield = 0; uint32_t uiByteOffset = 0; - uint8_t ucCurrentByte = **ppucDataBuffer_; + unsigned char ucCurrentByte = **ppucData_; - // Iterate over each bit, adding the bit to the 64-bit return value if it is set. + // Iterate over each bit, adding the bit to the return value if it is set. for (uint32_t uiBitsConsumed = 0; uiBitsConsumed < uiBitsInBitfield_; uiBitsConsumed++) { - if ((ucCurrentByte & (1UL << uiMyBitOffset)) != 0U) { ulBitfield |= (1ULL << uiBitsConsumed); } + assert(uiBitOffset_ < 8); + + if ((ucCurrentByte & (1UL << uiBitOffset_)) != 0) { ullBitfield |= 1ULL << uiBitsConsumed; } // Rollover to the next byte when we reach the end of the current byte. - uiMyBitOffset++; - if (uiMyBitOffset == BITS_PER_BYTE) + if (++uiBitOffset_ == BITS_PER_BYTE) { - uiMyBitOffset = 0; + uiBitOffset_ = 0; uiByteOffset++; - if (uiBitsConsumed + 1 < uiBitsInBitfield_) { ucCurrentByte = *(*ppucDataBuffer_ + uiByteOffset); } + if (uiBitsConsumed + 1 < uiBitsInBitfield_) { ucCurrentByte = *(*ppucData_ + uiByteOffset); } } } - // Advance the data buffer we received by the amount of bytes that we moved. - *ppucDataBuffer_ += uiByteOffset; - return ulBitfield; + *ppucData_ += uiByteOffset; + + return static_cast(ullBitfield); } +// Explicit template instantiations +template uint64_t RangeDecompressor::ExtractBitfield(unsigned char**, uint32_t&, uint32_t&, uint32_t); + //------------------------------------------------------------------------------ //! The locktime can only report up to 131071ms (0x1FFFF). If this value is //! reached, the locktime must continue to increment. Once the saturated value @@ -351,13 +263,13 @@ uint64_t RangeDecompressor::GetBitfieldFromBuffer(uint8_t** ppucDataBuffer_, uin //! observation, and may not be a true representation of the time the //! observation has actually been locked. //------------------------------------------------------------------------------ -float RangeDecompressor::DetermineRangeCmp2ObservationLockTime(const MetaDataStruct& stMetaData_, uint32_t uiLockTimeBits_, - ChannelTrackingStatusStruct::SATELLITE_SYSTEM eSystem_, - ChannelTrackingStatusStruct::SIGNAL_TYPE eSignal_, uint16_t usPRN_) +float RangeDecompressor::GetRangeCmp2LockTime(const MetaDataStruct& stMetaData_, uint32_t uiLockTimeBits_, + ChannelTrackingStatus::SATELLITE_SYSTEM eSystem_, ChannelTrackingStatus::SIGNAL_TYPE eSignal_, + uint16_t usPRN_) { auto fLocktimeMilliseconds = static_cast(uiLockTimeBits_); - RangeCmp2LockTimeInfoStruct& stLocktimeInfo = + RangeCmp2LockTimeInfo& stLocktimeInfo = ammmMyRangeCmp2LockTimes[static_cast(stMetaData_.eMeasurementSource)][eSystem_][eSignal_][static_cast(usPRN_)]; if (uiLockTimeBits_ == (RC2_SIG_LOCKTIME_MASK >> RC2_SIG_LOCKTIME_SHIFT)) { @@ -366,18 +278,15 @@ float RangeDecompressor::DetermineRangeCmp2ObservationLockTime(const MetaDataStr { fLocktimeMilliseconds += static_cast(stMetaData_.dMilliseconds - stLocktimeInfo.dLockTimeSaturatedMilliseconds); } - else // If the locktime is not already saturated, store this information if this - // observation is seen again. + // If the locktime is not already saturated, store this information if this observation is seen again. + else { stLocktimeInfo.dLockTimeSaturatedMilliseconds = stMetaData_.dMilliseconds; stLocktimeInfo.bLockTimeSaturated = true; } } - else if (stLocktimeInfo.bLockTimeSaturated) // If the locktime marked as saturated, but is not reported - // as such from the RANGECMP2 message, clear the flag. - { - stLocktimeInfo.bLockTimeSaturated = false; - } + // If the locktime marked as saturated, but is not reported as such from the RANGECMP2 message, clear the flag. + else if (stLocktimeInfo.bLockTimeSaturated) { stLocktimeInfo.bLockTimeSaturated = false; } return fLocktimeMilliseconds / SEC_TO_MILLI_SEC; } @@ -388,28 +297,28 @@ float RangeDecompressor::DetermineRangeCmp2ObservationLockTime(const MetaDataStr //! precise time at which lock was acquired can result in initially misledaing //! locktimes. There are a number of cases which may occur when decompressing //! the locktime bitfields from a RANGECMP4 observation: -//! NOTE: See afTheRangeCmp4LockTimeValues for bitfield:locktime translations. -//! 1. The locktime bitfield is b0000: +//! NOTE: See lockTime for ullBitfield:locktime translations. +//! 1. The locktime ullBitfield is b0000: //! - This is the simplest case. The locktime will increase with the time //! reported by the message header. -//! 2. The locktime bitfield is b1111: +//! 2. The locktime ullBitfield is b1111: //! - There is no way to determine when the locktime reached that range, //! so as for case 1, the locktime will increase with the time reported //! by the message header. -//! 3. The locktime bitfield is any value from b0001 to b1110: +//! 3. The locktime ullBitfield is any value from b0001 to b1110: //! - This case is the most complex. Because there is no guarantee that //! the current locktime reported has just transitioned to the lower -//! boundary of the bitfield representation, it must be stated that the +//! boundary of the ullBitfield representation, it must be stated that the //! locktime is relative, and not absolute. Only when a change in the -//! bitfield is detected can it be guaranteed the the locktime is +//! ullBitfield is detected can it be guaranteed the the locktime is //! absolute. At this point, the locktime can jump or slip to adjust -//! to the newly found bitfield. After the jump or slip occurs, the -//! locktime must not change again, as the lower bitfield values will +//! to the newly found ullBitfield. After the jump or slip occurs, the +//! locktime must not change again, as the lower ullBitfield values will //! produce the highest degree of accuracy. -//! - For example, if the locktime bitfield is b0111, the observation has +//! - For example, if the locktime ullBitfield is b0111, the observation has //! been locked for at least 1024ms. However it is possible the message //! was produced when the observation was locked for 1800ms. This means -//! a 776ms discrepancy exsists between what the RangeDecompressor tracks +//! a 776ms discrepancy exists between what the RangeDecompressor tracks //! and what is reported in the RANGECMP messages. When this discrepancy //! is detected, the locktime can jump to match what the RANGECMP message //! reports. Thus, the distinction between "relative" and "absolute" @@ -425,7 +334,7 @@ float RangeDecompressor::DetermineRangeCmp2ObservationLockTime(const MetaDataStr //! | / //! | / //! | / -//! | / < At this point a bitfield change +//! | / < At this point a ullBitfield change //! | *| was found. The transition from //! | * | "relative" to "absolute" time //! | * | results in a locktime jump as @@ -441,40 +350,47 @@ float RangeDecompressor::DetermineRangeCmp2ObservationLockTime(const MetaDataStr //! +--------------------------------------------------------------------> //! Header time (t) //------------------------------------------------------------------------------ -float RangeDecompressor::DetermineRangeCmp4ObservationLockTime(const MetaDataStruct& stMetaData_, uint8_t ucLockTimeBits_, - ChannelTrackingStatusStruct::SATELLITE_SYSTEM eSystem_, - ChannelTrackingStatusStruct::SIGNAL_TYPE eSignal_, uint32_t uiPRN_) +float RangeDecompressor::GetRangeCmp4LockTime(const MetaDataStruct& stMetaData_, uint8_t ucLockTimeBits_, + ChannelTrackingStatus::SATELLITE_SYSTEM eSystem_, ChannelTrackingStatus::SIGNAL_TYPE eSignal_, + uint32_t uiPRN_) { - // Store the locktime if it is different then the once we have currently. - RangeCmp4LocktimeInfoStruct& stLocktimeInfo = + //----------------------------------------------------------------------- + //! List of pre-defined floats used as translations for RANGECMP4 lock + //! time values defined in the RANGECMP4 documentation: + //! https://docs.novatel.com/OEM7/Content/Logs/RANGECMP4.htm?Highlight=Range#Lock + //! NOTE: These values are the lower bound of the range representations. + //! For more information on decompressing locktime bitfields, see the + //! comment block above RangeDecompressor::DetermineRangeCmp4ObservationLocktime(). + //----------------------------------------------------------------------- + constexpr std::array lockTime = {0.0F, 16.0F, 32.0F, 64.0F, 128.0F, 256.0F, 512.0F, 1024.0F, + 2048.0F, 4096.0F, 8192.0F, 16384.0F, 32768.0F, 65536.0F, 131072.0F, 262144.0F}; + + // Store the locktime if it is different than the once we have currently. + RangeCmp4LocktimeInfo& stLocktimeInfo = ammmMyRangeCmp4LockTimes[static_cast(stMetaData_.eMeasurementSource)][eSystem_][eSignal_][uiPRN_]; - // Is the locktime relative and has a bitfield change been found? + // Is the locktime relative and has a ullBitfield change been found? if ((!stLocktimeInfo.bLocktimeAbsolute) && (ucLockTimeBits_ != stLocktimeInfo.ucLocktimeBits)) { - // If the locktime bits are 0, we can immediately set this as absolute locktime. - // Or, if the locktime bits stored for this observation are non-default (0xFF), - // this is a transition point from relative to absolute locktime. + // Set locktime as absolute if bits are 0 or transitioning from relative to absolute. if (ucLockTimeBits_ == 0 || (stLocktimeInfo.ucLocktimeBits != 0xFF && ucLockTimeBits_ > stLocktimeInfo.ucLocktimeBits)) { stLocktimeInfo.bLocktimeAbsolute = true; - // Record any locktime jump or slip for records. - double dLocktimeChangeMilliseconds = afTheRangeCmp4LockTimeValues[ucLockTimeBits_] - stLocktimeInfo.dLocktimeMilliseconds; - if (fabs(dLocktimeChangeMilliseconds) > std::numeric_limits::epsilon()) + double dLocktimeDeltaMs = std::abs(lockTime[ucLockTimeBits_] - stLocktimeInfo.dLocktimeMilliseconds); + if (dLocktimeDeltaMs > std::numeric_limits::epsilon()) { - pclMyLogger->warn("Detected a locktime jump of {}ms at time {}w, {}ms. SYSTEM: {}, SIGNAL: {}, " - "PRN: {}.", - std::abs(dLocktimeChangeMilliseconds), stMetaData_.usWeek, stMetaData_.dMilliseconds, - static_cast(eSystem_), static_cast(eSignal_), uiPRN_); + pclMyLogger->warn("Detected a locktime jump of {}ms at time {}w, {}ms. SYSTEM: {}, SIGNAL: {}, PRN: {}.", dLocktimeDeltaMs, + stMetaData_.usWeek, stMetaData_.dMilliseconds, static_cast(eSystem_), static_cast(eSignal_), + uiPRN_); } } - // Record the last bit change and the bitfield that was changed to. + // Record the last bit change and the ullBitfield that was changed to. stLocktimeInfo.dLastBitfieldChangeMilliseconds = stMetaData_.dMilliseconds; stLocktimeInfo.ucLocktimeBits = ucLockTimeBits_; - } // If the locktime is absolute and the locktime bits have decreased, there was likely an - // outage. + } + // If the locktime is absolute and the locktime bits have decreased, there was likely an outage. else if (ucLockTimeBits_ < stLocktimeInfo.ucLocktimeBits) { // In the event of an outage of any kind, reset the locktime to relative. @@ -482,26 +398,22 @@ float RangeDecompressor::DetermineRangeCmp4ObservationLockTime(const MetaDataStr stLocktimeInfo.dLastBitfieldChangeMilliseconds = stMetaData_.dMilliseconds; stLocktimeInfo.ucLocktimeBits = ucLockTimeBits_; - // Record the slip - pclMyLogger->warn("Detected a locktime slip (perhaps caused by an outage) of {}ms at time {}w, {}ms. " - "SYSTEM: {}, SIGNAL: {}, PRN: {}.", - (stLocktimeInfo.dLocktimeMilliseconds - afTheRangeCmp4LockTimeValues[stLocktimeInfo.ucLocktimeBits]), stMetaData_.usWeek, + pclMyLogger->warn("Detected a locktime slip (perhaps caused by an outage) of {}ms at time {}w, {}ms. SYSTEM: {}, SIGNAL: {}, PRN: {}.", + stLocktimeInfo.dLocktimeMilliseconds - lockTime[stLocktimeInfo.ucLocktimeBits], stMetaData_.usWeek, stMetaData_.dMilliseconds, static_cast(eSystem_), static_cast(eSignal_), uiPRN_); } else { - // If the locktime is absolute, and the bits have not changed after a change would be - // expected, reset the time since last bitfield change as there may have been an outage. - if ((stMetaData_.dMilliseconds - stLocktimeInfo.dLastBitfieldChangeMilliseconds) > (2 * afTheRangeCmp4LockTimeValues[ucLockTimeBits_])) + // If the locktime is absolute and the ullBitfield hasn't changed within the expected time, reset the last change time + if (stMetaData_.dMilliseconds - stLocktimeInfo.dLastBitfieldChangeMilliseconds > 2 * lockTime[ucLockTimeBits_]) { - pclMyLogger->warn("Expected a bit change much sooner at time {}w, {}ms. SYSTEM: {}, SIGNAL: {}, PRN: " - "{}.", - stMetaData_.usWeek, stMetaData_.dMilliseconds, static_cast(eSystem_), static_cast(eSignal_), uiPRN_); stLocktimeInfo.dLastBitfieldChangeMilliseconds = stMetaData_.dMilliseconds; + pclMyLogger->warn("Expected a bit change much sooner at time {}w, {}ms. SYSTEM: {}, SIGNAL: {}, PRN: {}.", stMetaData_.usWeek, + stMetaData_.dMilliseconds, static_cast(eSystem_), static_cast(eSignal_), uiPRN_); } } stLocktimeInfo.dLocktimeMilliseconds = - (stMetaData_.dMilliseconds - stLocktimeInfo.dLastBitfieldChangeMilliseconds) + afTheRangeCmp4LockTimeValues[stLocktimeInfo.ucLocktimeBits]; + stMetaData_.dMilliseconds - stLocktimeInfo.dLastBitfieldChangeMilliseconds + lockTime[stLocktimeInfo.ucLocktimeBits]; return static_cast(stLocktimeInfo.dLocktimeMilliseconds) / SEC_TO_MILLI_SEC; } @@ -510,40 +422,38 @@ float RangeDecompressor::DetermineRangeCmp4ObservationLockTime(const MetaDataStr //! provided reference block struct. //------------------------------------------------------------------------------ template -void RangeDecompressor::DecompressReferenceBlock(uint8_t** ppucDataPointer_, RangeCmp4MeasurementSignalBlockStruct& stReferenceBlock_, - MEASUREMENT_SOURCE eMeasurementSource_) +void RangeDecompressor::DecompressReferenceBlock(unsigned char** ppucData_, uint32_t& uiBytesLeft_, uint32_t& uiBitOffset_, + RangeCmp4MeasurementSignalBlock& stRefBlock_, MEASUREMENT_SOURCE eMeasurementSource_) { - // These fields are the same size regardless of the reference block being primary or secondary. - stReferenceBlock_.bParityKnown = static_cast(GetBitfieldFromBuffer(ppucDataPointer_, RC4_SIG_BLK_PARITY_FLAG_BITS)); - stReferenceBlock_.bHalfCycleAdded = static_cast(GetBitfieldFromBuffer(ppucDataPointer_, RC4_SIG_BLK_HALF_CYCLE_BITS)); - stReferenceBlock_.fCNo = static_cast(GetBitfieldFromBuffer(ppucDataPointer_, RC4_SIG_BLK_CNO_BITS)) * RC4_SIG_BLK_CNO_SCALE_FACTOR; - stReferenceBlock_.ucLockTimeBitfield = static_cast(GetBitfieldFromBuffer(ppucDataPointer_, RC4_SIG_BLK_LOCK_TIME_BITS)); - stReferenceBlock_.ucPSRBitfield = static_cast(GetBitfieldFromBuffer(ppucDataPointer_, RC4_SIG_BLK_PSR_STDDEV_BITS)); - stReferenceBlock_.ucADRBitfield = static_cast(GetBitfieldFromBuffer(ppucDataPointer_, RC4_SIG_BLK_ADR_STDDEV_BITS)); - - auto llPSRBitfield = static_cast(GetBitfieldFromBuffer(ppucDataPointer_, RC4_RBLK_PSR_BITS[bSecondary])); + stRefBlock_.bParityKnown = ExtractBitfield(ppucData_, uiBytesLeft_, uiBitOffset_, RC4_SIG_BLK_PARITY_FLAG_BITS); + stRefBlock_.bHalfCycleAdded = ExtractBitfield(ppucData_, uiBytesLeft_, uiBitOffset_, RC4_SIG_BLK_HALF_CYCLE_BITS); + stRefBlock_.fCNo = ExtractBitfield(ppucData_, uiBytesLeft_, uiBitOffset_, RC4_SIG_BLK_CNO_BITS) * RC4_SIG_BLK_CNO_SCALE_FACTOR; + stRefBlock_.ucLockTimeBitfield = ExtractBitfield(ppucData_, uiBytesLeft_, uiBitOffset_, RC4_SIG_BLK_LOCK_TIME_BITS); + stRefBlock_.ucPSRBitfield = ExtractBitfield(ppucData_, uiBytesLeft_, uiBitOffset_, RC4_SIG_BLK_PSR_STDDEV_BITS); + stRefBlock_.ucADRBitfield = ExtractBitfield(ppucData_, uiBytesLeft_, uiBitOffset_, RC4_SIG_BLK_ADR_STDDEV_BITS); + + auto llPSRBitfield = ExtractBitfield(ppucData_, uiBytesLeft_, uiBitOffset_, RC4_RBLK_PSR_BITS[bSecondary]); if constexpr (bSecondary) { if (llPSRBitfield & RC4_SSIG_RBLK_PSR_SIGNBIT_MASK) { llPSRBitfield |= RC4_SSIG_RBLK_PSR_SIGNEXT_MASK; } } - auto iPhaseRangeBitfield = static_cast(GetBitfieldFromBuffer(ppucDataPointer_, RC4_RBLK_PHASERANGE_BITS[bSecondary])); + auto iPhaseRangeBitfield = ExtractBitfield(ppucData_, uiBytesLeft_, uiBitOffset_, RC4_RBLK_PHASERANGE_BITS[bSecondary]); if (iPhaseRangeBitfield & RC4_RBLK_PHASERANGE_SIGNBIT_MASK) { iPhaseRangeBitfield |= RC4_RBLK_PHASERANGE_SIGNEXT_MASK; } - auto iDopplerBitfield = static_cast(GetBitfieldFromBuffer(ppucDataPointer_, RC4_RBLK_DOPPLER_BITS[bSecondary])); + auto iDopplerBitfield = ExtractBitfield(ppucData_, uiBytesLeft_, uiBitOffset_, RC4_RBLK_DOPPLER_BITS[bSecondary]); if (iDopplerBitfield & RC4_RBLK_DOPPLER_SIGNBIT_MASK[bSecondary]) { iDopplerBitfield |= RC4_RBLK_DOPPLER_SIGNEXT_MASK[bSecondary]; } - stReferenceBlock_.bValidPSR = llPSRBitfield != RC4_RBLK_INVALID_PSR[bSecondary]; - stReferenceBlock_.dPSR = static_cast(llPSRBitfield) * RC4_SIG_BLK_PSR_SCALE_FACTOR + - (bSecondary ? astMyLastPrimaryReferenceBlocks[static_cast(eMeasurementSource_)].dPSR : 0); - stReferenceBlock_.bValidPhaseRange = iPhaseRangeBitfield != RC4_SIG_RBLK_INVALID_PHASERANGE; - stReferenceBlock_.dPhaseRange = static_cast(iPhaseRangeBitfield) * RC4_SIG_BLK_PHASERANGE_SCALE_FACTOR + stReferenceBlock_.dPSR; - stReferenceBlock_.bValidDoppler = iDopplerBitfield != RC4_PSIG_RBLK_INVALID_DOPPLER; - stReferenceBlock_.dDoppler = static_cast(iDopplerBitfield) * RC4_SIG_BLK_DOPPLER_SCALE_FACTOR + - (bSecondary ? astMyLastPrimaryReferenceBlocks[static_cast(eMeasurementSource_)].dDoppler : 0); + stRefBlock_.bValidPSR = llPSRBitfield != RC4_RBLK_INVALID_PSR[bSecondary]; + stRefBlock_.dPSR = static_cast(llPSRBitfield) * RC4_SIG_BLK_PSR_SCALE_FACTOR + + (bSecondary ? astMyLastPrimaryReferenceBlocks[static_cast(eMeasurementSource_)].dPSR : 0); + stRefBlock_.bValidPhaseRange = iPhaseRangeBitfield != RC4_SIG_RBLK_INVALID_PHASERANGE; + stRefBlock_.dPhaseRange = static_cast(iPhaseRangeBitfield) * RC4_SIG_BLK_PHASERANGE_SCALE_FACTOR + stRefBlock_.dPSR; + stRefBlock_.bValidDoppler = iDopplerBitfield != RC4_PSIG_RBLK_INVALID_DOPPLER; + stRefBlock_.dDoppler = static_cast(iDopplerBitfield) * RC4_SIG_BLK_DOPPLER_SCALE_FACTOR + + (bSecondary ? astMyLastPrimaryReferenceBlocks[static_cast(eMeasurementSource_)].dDoppler : 0); if constexpr (!bSecondary) { // For subsequent blocks, we're going to need to store this primary block. - memcpy(&astMyLastPrimaryReferenceBlocks[static_cast(eMeasurementSource_)], &stReferenceBlock_, - sizeof(RangeCmp4MeasurementSignalBlockStruct)); + memcpy(&astMyLastPrimaryReferenceBlocks[static_cast(eMeasurementSource_)], &stRefBlock_, sizeof(RangeCmp4MeasurementSignalBlock)); } } @@ -552,51 +462,69 @@ void RangeDecompressor::DecompressReferenceBlock(uint8_t** ppucDataPointer_, Ran //! provided reference block struct, but must be given the appropriate //! reference block from the same RANGECMP4 message. //------------------------------------------------------------------------------ -template -void RangeDecompressor::DecompressDifferentialBlock(uint8_t** ppucDataPointer_, RangeCmp4MeasurementSignalBlockStruct& stDifferentialBlock_, - const RangeCmp4MeasurementSignalBlockStruct& stReferenceBlock_, double dSecondOffset_) +template +void RangeDecompressor::DecompressDifferentialBlock(unsigned char** ppucData_, uint32_t& uiBytesLeft_, uint32_t& uiBitOffset_, + RangeCmp4MeasurementSignalBlock& stDiffBlock_, const RangeCmp4MeasurementSignalBlock& stRefBlock_, + double dSecondOffset_) { - // These fields are the same size regardless of the reference block being primary or secondary. - stDifferentialBlock_.bParityKnown = static_cast(GetBitfieldFromBuffer(ppucDataPointer_, RC4_SIG_BLK_PARITY_FLAG_BITS)); - stDifferentialBlock_.bHalfCycleAdded = static_cast(GetBitfieldFromBuffer(ppucDataPointer_, RC4_SIG_BLK_HALF_CYCLE_BITS)); - stDifferentialBlock_.fCNo = static_cast(GetBitfieldFromBuffer(ppucDataPointer_, RC4_SIG_BLK_CNO_BITS)) * RC4_SIG_BLK_CNO_SCALE_FACTOR; - stDifferentialBlock_.ucLockTimeBitfield = static_cast(GetBitfieldFromBuffer(ppucDataPointer_, RC4_SIG_BLK_LOCK_TIME_BITS)); - stDifferentialBlock_.ucPSRBitfield = static_cast(GetBitfieldFromBuffer(ppucDataPointer_, RC4_SIG_BLK_PSR_STDDEV_BITS)); - stDifferentialBlock_.ucADRBitfield = static_cast(GetBitfieldFromBuffer(ppucDataPointer_, RC4_SIG_BLK_ADR_STDDEV_BITS)); - - auto iPSRBitfield = static_cast(GetBitfieldFromBuffer(ppucDataPointer_, RC4_SIG_DBLK_PSR_BITS)); + stDiffBlock_.bParityKnown = ExtractBitfield(ppucData_, uiBytesLeft_, uiBitOffset_, RC4_SIG_BLK_PARITY_FLAG_BITS); + stDiffBlock_.bHalfCycleAdded = ExtractBitfield(ppucData_, uiBytesLeft_, uiBitOffset_, RC4_SIG_BLK_HALF_CYCLE_BITS); + stDiffBlock_.fCNo = ExtractBitfield(ppucData_, uiBytesLeft_, uiBitOffset_, RC4_SIG_BLK_CNO_BITS) * RC4_SIG_BLK_CNO_SCALE_FACTOR; + stDiffBlock_.ucLockTimeBitfield = ExtractBitfield(ppucData_, uiBytesLeft_, uiBitOffset_, RC4_SIG_BLK_LOCK_TIME_BITS); + stDiffBlock_.ucPSRBitfield = ExtractBitfield(ppucData_, uiBytesLeft_, uiBitOffset_, RC4_SIG_BLK_PSR_STDDEV_BITS); + stDiffBlock_.ucADRBitfield = ExtractBitfield(ppucData_, uiBytesLeft_, uiBitOffset_, RC4_SIG_BLK_ADR_STDDEV_BITS); + + auto iPSRBitfield = ExtractBitfield(ppucData_, uiBytesLeft_, uiBitOffset_, RC4_SIG_DBLK_PSR_BITS); if (iPSRBitfield & RC4_SIG_DBLK_PSR_SIGNBIT_MASK) { iPSRBitfield |= RC4_SIG_DBLK_PSR_SIGNEXT_MASK; } - auto iPhaseRangeBitfield = static_cast(GetBitfieldFromBuffer(ppucDataPointer_, RC4_SIG_DBLK_PHASERANGE_BITS)); + auto iPhaseRangeBitfield = ExtractBitfield(ppucData_, uiBytesLeft_, uiBitOffset_, RC4_SIG_DBLK_PHASERANGE_BITS); if (iPhaseRangeBitfield & RC4_SIG_DBLK_PHASERANGE_SIGNBIT_MASK) { iPhaseRangeBitfield |= RC4_SIG_DBLK_PHASERANGE_SIGNEXT_MASK; } - auto iDopplerBitfield = static_cast(GetBitfieldFromBuffer(ppucDataPointer_, RC4_DBLK_DOPPLER_BITS[bIsSecondary])); - if (iDopplerBitfield & RC4_DBLK_DOPPLER_SIGNBIT_MASK[bIsSecondary]) { iDopplerBitfield |= RC4_DBLK_DOPPLER_SIGNEXT_MASK[bIsSecondary]; } - - stDifferentialBlock_.bValidPSR = iPSRBitfield != RC4_SIG_DBLK_INVALID_PSR; - stDifferentialBlock_.dPSR = - static_cast(iPSRBitfield) * RC4_SIG_BLK_PSR_SCALE_FACTOR + stReferenceBlock_.dPSR + (stReferenceBlock_.dDoppler * dSecondOffset_); - stDifferentialBlock_.bValidPhaseRange = iPhaseRangeBitfield != RC4_SIG_DBLK_INVALID_PHASERANGE; - stDifferentialBlock_.dPhaseRange = static_cast(iPhaseRangeBitfield) * RC4_SIG_BLK_PHASERANGE_SCALE_FACTOR + - stReferenceBlock_.dPhaseRange + (stReferenceBlock_.dDoppler * dSecondOffset_); - stDifferentialBlock_.bValidDoppler = iDopplerBitfield != RC4_DBLK_INVALID_DOPPLER[bIsSecondary]; - stDifferentialBlock_.dDoppler = static_cast(iDopplerBitfield) * RC4_SIG_BLK_DOPPLER_SCALE_FACTOR + stReferenceBlock_.dDoppler; + auto iDopplerBitfield = ExtractBitfield(ppucData_, uiBytesLeft_, uiBitOffset_, RC4_DBLK_DOPPLER_BITS[bSecondary]); + if (iDopplerBitfield & RC4_DBLK_DOPPLER_SIGNBIT_MASK[bSecondary]) { iDopplerBitfield |= RC4_DBLK_DOPPLER_SIGNEXT_MASK[bSecondary]; } + + stDiffBlock_.bValidPSR = iPSRBitfield != RC4_SIG_DBLK_INVALID_PSR; + stDiffBlock_.dPSR = static_cast(iPSRBitfield) * RC4_SIG_BLK_PSR_SCALE_FACTOR + stRefBlock_.dPSR + (stRefBlock_.dDoppler * dSecondOffset_); + stDiffBlock_.bValidPhaseRange = iPhaseRangeBitfield != RC4_SIG_DBLK_INVALID_PHASERANGE; + stDiffBlock_.dPhaseRange = static_cast(iPhaseRangeBitfield) * RC4_SIG_BLK_PHASERANGE_SCALE_FACTOR + stRefBlock_.dPhaseRange + + stRefBlock_.dDoppler * dSecondOffset_; + stDiffBlock_.bValidDoppler = iDopplerBitfield != RC4_DBLK_INVALID_DOPPLER[bSecondary]; + stDiffBlock_.dDoppler = static_cast(iDopplerBitfield) * RC4_SIG_BLK_DOPPLER_SCALE_FACTOR + stRefBlock_.dDoppler; } //------------------------------------------------------------------------------ //! Populates a provided RangeData structure from the RANGECMP4 blocks //! provided. //------------------------------------------------------------------------------ -void RangeDecompressor::PopulateNextRangeData(RangeDataStruct& stRangeData_, const RangeCmp4MeasurementSignalBlockStruct& stBlock_, - const MetaDataStruct& stMetaData_, const ChannelTrackingStatusStruct& stChannelTrackingStatus_, - uint32_t uiPRN_, char cGLONASSFrequencyNumber_) +void RangeDecompressor::PopulateNextRangeData(RangeData& stRangeData_, const RangeCmp4MeasurementSignalBlock& stBlock_, + const MetaDataStruct& stMetaData_, const ChannelTrackingStatus& stChannelStatus_, uint32_t uiPRN_, + char cGLONASSFrequencyNumber_) { + //----------------------------------------------------------------------- + //! List of pre-defined floats used as translations for RANGECMP4 PSR + //! standard deviation values defined in the RANGECMP4 documentation: + //! https://docs.novatel.com/OEM7/Content/Logs/RANGECMP4.htm?Highlight=Range#Pseudora + //----------------------------------------------------------------------- + constexpr std::array stdDevPsrScaling = {0.020F, 0.030F, 0.045F, 0.066F, 0.099F, 0.148F, 0.220F, 0.329F, + 0.491F, 0.732F, 1.092F, 1.629F, 2.430F, 3.625F, 5.409F, 5.409F}; + + //----------------------------------------------------------------------- + //! List of pre-defined floats used as translations for RANGECMP4 ADR + //! standard deviation values defined in the RANGECMP4 documentation: + //! https://docs.novatel.com/OEM7/Content/Logs/RANGECMP4.htm?Highlight=Range#ADR + //! Note: LSD have been removed to reduce rouning errors in the range log. + //! ADR STD is only 3 decimal places and could round up causing values + //! to be greater than values in the table. + //----------------------------------------------------------------------- + constexpr std::array stdDevAdrScaling = {0.003F, 0.005F, 0.007F, 0.009F, 0.012F, 0.016F, 0.022F, 0.029F, + 0.039F, 0.052F, 0.070F, 0.093F, 0.124F, 0.166F, 0.222F, 0.222F}; + double dSignalWavelength = - GetSignalWavelength(stChannelTrackingStatus_, static_cast(cGLONASSFrequencyNumber_ - GLONASS_FREQUENCY_NUMBER_OFFSET)); + GetSignalWavelength(stChannelStatus_, static_cast(cGLONASSFrequencyNumber_ - GLONASS_FREQUENCY_NUMBER_OFFSET)); //! Some logic for PRN offsets based on the constellation. See documentation: //! https://docs.novatel.com/OEM7/Content/Logs/RANGECMP4.htm#Measurem - switch (stChannelTrackingStatus_.eSatelliteSystem) + switch (stChannelStatus_.eSatelliteSystem) { - case ChannelTrackingStatusStruct::SATELLITE_SYSTEM::GLONASS: + case ChannelTrackingStatus::SATELLITE_SYSTEM::GLONASS: // If ternary returns true, documentation suggests we should save this PRN as // GLONASS_SLOT_UNKNOWN_UPPER_LIMIT - cGLONASSFrequencyNumber_. However, this // would output the PRN as an actual valid Slot ID, which is not true. We will @@ -605,18 +533,18 @@ void RangeDecompressor::PopulateNextRangeData(RangeDataStruct& stRangeData_, con ? 0 : static_cast(uiPRN_) + GLONASS_SLOT_OFFSET - 1; break; - case ChannelTrackingStatusStruct::SATELLITE_SYSTEM::SBAS: + case ChannelTrackingStatus::SATELLITE_SYSTEM::SBAS: stRangeData_.usPRN = (SBAS_PRN_OFFSET_120_LOWER_LIMIT <= uiPRN_ && uiPRN_ <= SBAS_PRN_OFFSET_120_UPPER_LIMIT) ? static_cast(uiPRN_ + SBAS_PRN_OFFSET_120 - 1) : (SBAS_PRN_OFFSET_130_LOWER_LIMIT <= uiPRN_ && uiPRN_ <= SBAS_PRN_OFFSET_130_UPPER_LIMIT) ? static_cast(uiPRN_ + SBAS_PRN_OFFSET_130 - 1) : 0; break; - case ChannelTrackingStatusStruct::SATELLITE_SYSTEM::QZSS: stRangeData_.usPRN = static_cast(uiPRN_ + QZSS_PRN_OFFSET - 1); break; + case ChannelTrackingStatus::SATELLITE_SYSTEM::QZSS: stRangeData_.usPRN = static_cast(uiPRN_ + QZSS_PRN_OFFSET - 1); break; default: stRangeData_.usPRN = static_cast(uiPRN_); break; } - if (stChannelTrackingStatus_.eSatelliteSystem != ChannelTrackingStatusStruct::SATELLITE_SYSTEM::GLONASS && stRangeData_.usPRN == 0) + if (stChannelStatus_.eSatelliteSystem != ChannelTrackingStatus::SATELLITE_SYSTEM::GLONASS && stRangeData_.usPRN == 0) { throw std::runtime_error("PopulateNextRangeData(): PRN outside of limits"); } @@ -624,47 +552,52 @@ void RangeDecompressor::PopulateNextRangeData(RangeDataStruct& stRangeData_, con // any fields flagged as invalid are set to NaN and appear in the log as such stRangeData_.sGLONASSFrequency = static_cast(static_cast(cGLONASSFrequencyNumber_)); stRangeData_.dPSR = stBlock_.bValidPSR ? stBlock_.dPSR : std::numeric_limits::quiet_NaN(); - stRangeData_.fPSRStdDev = afTheRangeCmp4PSRStdDevValues[stBlock_.ucPSRBitfield]; - stRangeData_.dADR = - stBlock_.bValidPhaseRange ? MAGIC_NEGATE * (stBlock_.dPhaseRange / dSignalWavelength) : std::numeric_limits::quiet_NaN(); - stRangeData_.fADRStdDev = afTheRangeCmp4ADRStdDevValues[stBlock_.ucADRBitfield]; + stRangeData_.fPSRStdDev = stdDevPsrScaling[stBlock_.ucPSRBitfield]; + stRangeData_.dADR = stBlock_.bValidPhaseRange ? -stBlock_.dPhaseRange / dSignalWavelength : std::numeric_limits::quiet_NaN(); + stRangeData_.fADRStdDev = stdDevAdrScaling[stBlock_.ucADRBitfield]; stRangeData_.fDopplerFrequency = - stBlock_.bValidDoppler ? static_cast(MAGIC_NEGATE * (stBlock_.dDoppler / dSignalWavelength)) : std::numeric_limits::quiet_NaN(); + stBlock_.bValidDoppler ? static_cast(-stBlock_.dDoppler / dSignalWavelength) : std::numeric_limits::quiet_NaN(); stRangeData_.fCNo = stBlock_.fCNo; - stRangeData_.fLockTime = DetermineRangeCmp4ObservationLockTime( - stMetaData_, stBlock_.ucLockTimeBitfield, stChannelTrackingStatus_.eSatelliteSystem, stChannelTrackingStatus_.eSignalType, uiPRN_); - stRangeData_.uiChannelTrackingStatus = stChannelTrackingStatus_.GetAsWord(); + stRangeData_.fLockTime = + GetRangeCmp4LockTime(stMetaData_, stBlock_.ucLockTimeBitfield, stChannelStatus_.eSatelliteSystem, stChannelStatus_.eSignalType, uiPRN_); + stRangeData_.uiChannelTrackingStatus = stChannelStatus_.GetAsWord(); } //------------------------------------------------------------------------------ //! Convert a RANGECMP message into RANGE message. //------------------------------------------------------------------------------ -void RangeDecompressor::RangeCmpToRange(const RangeCmpStruct& stRangeCmpMessage_, RangeStruct& stRangeMessage_) +void RangeDecompressor::RangeCmpToRange(const RangeCmp& stRangeCmpMessage_, Range& stRangeMessage_) { - uint32_t uiChannelTrackingStatus = 0; + //----------------------------------------------------------------------- + //! Table used to expand the scaled Pseudorange STDs. This is defined + //! in the RANGECMP documentation: + //! https://docs.novatel.com/OEM7/Content/Logs/RANGECMP.htm?Highlight=rangecmp#StdDevPSRValues + //----------------------------------------------------------------------- + constexpr std::array stdDevPsrScaling = {0.050F, 0.075F, 0.113F, 0.169F, 0.253F, 0.380F, 0.570F, 0.854F, + 1.281F, 2.375F, 4.750F, 9.500F, 19.000F, 38.000F, 76.000F, 152.000F}; + stRangeMessage_.uiNumberOfObservations = stRangeCmpMessage_.uiNumberOfObservations; // Decompress each field from the RangeCmpData and put it into the respective RangeData. for (uint32_t uiRangeDataIndex = 0; uiRangeDataIndex < stRangeMessage_.uiNumberOfObservations; uiRangeDataIndex++) { - RangeDataStruct& stRangeData = stRangeMessage_.astRangeData[uiRangeDataIndex]; - const RangeCmpDataStruct& stRangeCmpData = stRangeCmpMessage_.astRangeData[uiRangeDataIndex]; + RangeData& stRangeData = stRangeMessage_.astRangeData[uiRangeDataIndex]; + const RangeCmpData& stRangeCmpData = stRangeCmpMessage_.astRangeData[uiRangeDataIndex]; // Grab the channel tracking status word and put it into our structure to use it later - uiChannelTrackingStatus = stRangeCmpData.uiChannelTrackingStatus; - ChannelTrackingStatusStruct stChannelTrackingStatus(uiChannelTrackingStatus); + ChannelTrackingStatus stChannelTrackingStatus(stRangeCmpData.uiChannelTrackingStatus); - stRangeData.uiChannelTrackingStatus = uiChannelTrackingStatus; + stRangeData.uiChannelTrackingStatus = stRangeCmpData.uiChannelTrackingStatus; stRangeData.usPRN = static_cast(stRangeCmpData.ucPRN); // Extend the sign - auto iTempDoppler = static_cast(stRangeCmpData.ulDopplerFrequencyPSRField & RC_DOPPLER_FREQUENCY_MASK); - if ((iTempDoppler & RC_DOPPLER_FREQUENCY_SIGNBIT_MASK) != 0U) { iTempDoppler |= RC_DOPPLER_FREQUENCY_SIGNEXT_MASK; } - stRangeData.fDopplerFrequency = static_cast(iTempDoppler / RC_DOPPLER_FREQUENCY_SCALE_FACTOR); + auto iDoppler = static_cast(stRangeCmpData.ulDopplerFrequencyPSRField & RC_DOPPLER_FREQUENCY_MASK); + if ((iDoppler & RC_DOPPLER_FREQUENCY_SIGNBIT_MASK) != 0) { iDoppler |= RC_DOPPLER_FREQUENCY_SIGNEXT_MASK; } + stRangeData.fDopplerFrequency = static_cast(iDoppler / RC_DOPPLER_FREQUENCY_SCALE_FACTOR); stRangeData.dPSR = static_cast(((stRangeCmpData.ulDopplerFrequencyPSRField & RC_PSR_MEASUREMENT_MASK) >> RC_PSR_MEASUREMENT_SHIFT) / RC_PSR_MEASUREMENT_SCALE_FACTOR); - stRangeData.fPSRStdDev = afTheRangeCmpPSRStdDevValues[static_cast(stRangeCmpData.ucStdDevPSRStdDevADR & RC_PSR_STDDEV_MASK)]; + stRangeData.fPSRStdDev = stdDevPsrScaling[stRangeCmpData.ucStdDevPSRStdDevADR & RC_PSR_STDDEV_MASK]; stRangeData.fADRStdDev = static_cast((RC_ADR_STDDEV_SCALE_OFFSET + ((stRangeCmpData.ucStdDevPSRStdDevADR & RC_ADR_STDDEV_MASK) >> RC_ADR_STDDEV_SHIFT)) / RC_ADR_STDDEV_SCALE_FACTOR); @@ -677,295 +610,262 @@ void RangeDecompressor::RangeCmpToRange(const RangeCmpStruct& stRangeCmpMessage_ stRangeData.dADR = static_cast(stRangeCmpData.uiADR) / RC_ADR_SCALE_FACTOR; double dADRRolls = ((stRangeData.dPSR / dWavelength) + stRangeData.dADR) / MAX_VALUE; - - // Round to the nearest integer. - dADRRolls += (dADRRolls <= 0) ? -0.5 : 0.5; - - stRangeData.dADR = stRangeData.dADR - (MAX_VALUE * static_cast(dADRRolls)); + stRangeData.dADR -= MAX_VALUE * static_cast(std::round(dADRRolls)); } } //------------------------------------------------------------------------------ -//! Convert a RANGECMP2 message into RANGE message. +// Convert a RANGECMP2 message into RANGE message. //------------------------------------------------------------------------------ -void RangeDecompressor::RangeCmp2ToRange(const RangeCmp2Struct& stRangeCmp2Message_, RangeStruct& stRangeMessage_, const MetaDataStruct& stMetaData_) +void RangeDecompressor::RangeCmp2ToRange(const RangeCmp2& stRangeCmp2Message_, Range& stRangeMessage_, const MetaDataStruct& stMetaData_) { + //----------------------------------------------------------------------- + //! Table used to expand the scaled Pseudorange STDs. This is defined + //! in the RANGECMP2 documentation: + //! https://docs.novatel.com/OEM7/Content/Logs/RANGECMP2.htm?Highlight=RANGECMP2#StdDevPSRScaling + //----------------------------------------------------------------------- + constexpr std::array stdDevPsrScaling = {0.020F, 0.030F, 0.045F, 0.066F, 0.099F, 0.148F, 0.220F, 0.329F, + 0.491F, 0.732F, 1.092F, 1.629F, 2.430F, 3.625F, 5.409F, 5.409F}; + + //----------------------------------------------------------------------- + //! Table used to expand the scaled Accumulated Doppler Range STDs. This + //! is defined in the RANGECMP2 documentation: + //! https://docs.novatel.com/OEM7/Content/Logs/RANGECMP2.htm?Highlight=RANGECMP2#StdDevADRScaling + //----------------------------------------------------------------------- + constexpr std::array stdDevAdrScaling = {0.00391F, 0.00521F, 0.00696F, 0.00929F, 0.01239F, 0.01654F, 0.02208F, 0.02947F, + 0.03933F, 0.05249F, 0.07006F, 0.09350F, 0.12480F, 0.16656F, 0.22230F, 0.22230F}; + stRangeMessage_.uiNumberOfObservations = 0; uint32_t uiRangeDataBytesDecompressed = 0; while (uiRangeDataBytesDecompressed < stRangeCmp2Message_.uiNumberOfRangeDataBytes) { - const auto& stRangeCmp2SatBlock = - reinterpret_cast(stRangeCmp2Message_.aucRangeData[uiRangeDataBytesDecompressed]); + const auto& stSatBlock = reinterpret_cast(stRangeCmp2Message_.aucRangeData[uiRangeDataBytesDecompressed]); // Decompress the Satellite Block - const auto eSatelliteSystem = - static_cast((stRangeCmp2SatBlock.ulCombinedField & RC2_SAT_SATELLITE_SYSTEM_ID_MASK) >> RC2_SAT_SATELLITE_SYSTEM_ID_SHIFT); - const auto ucNumberOfSignalBlocks = static_cast( - static_cast(stRangeCmp2SatBlock.ulCombinedField & RC2_SAT_NUM_SIGNAL_BLOCKS_BASE_MASK) >> RC2_SAT_NUM_SIGNAL_BLOCKS_BASE_SHIFT); + const auto eSystem = + static_cast((stSatBlock.ulCombinedField & RC2_SAT_SATELLITE_SYSTEM_ID_MASK) >> RC2_SAT_SATELLITE_SYSTEM_ID_SHIFT); + const auto ucSignalBlockCount = + static_cast((stSatBlock.ulCombinedField & RC2_SAT_NUM_SIGNAL_BLOCKS_BASE_MASK) >> RC2_SAT_NUM_SIGNAL_BLOCKS_BASE_SHIFT); // Extend the sign - auto iPSRBase = static_cast(static_cast(stRangeCmp2SatBlock.ulCombinedField & RC2_SAT_SATELLITE_PSR_BASE_MASK) >> - RC2_SAT_SATELLITE_PSR_BASE_SHIFT); - if ((iPSRBase & RC2_SAT_SATELLITE_PSR_BASE_SIGNBIT_MASK) != 0U) { iPSRBase |= RC2_SAT_SATELLITE_PSR_BASE_SIGNEXT_MASK; } + auto iPSRBase = static_cast((stSatBlock.ulCombinedField & RC2_SAT_SATELLITE_PSR_BASE_MASK) >> RC2_SAT_SATELLITE_PSR_BASE_SHIFT); + if ((iPSRBase & RC2_SAT_SATELLITE_PSR_BASE_SIGNBIT_MASK) != 0) { iPSRBase |= RC2_SAT_SATELLITE_PSR_BASE_SIGNEXT_MASK; } // Extend the sign - auto iDopplerBase = static_cast(static_cast(stRangeCmp2SatBlock.ulCombinedField & RC2_SAT_SATELLITE_DOPPLER_BASE_MASK) >> - RC2_SAT_SATELLITE_DOPPLER_BASE_SHIFT); - if ((iDopplerBase & RC2_SAT_SATELLITE_DOPPLER_BASE_SIGNBIT_MASK) != 0U) { iDopplerBase |= RC2_SAT_SATELLITE_DOPPLER_BASE_SIGNEXT_MASK; } + auto iDopplerBase = + static_cast((stSatBlock.ulCombinedField & RC2_SAT_SATELLITE_DOPPLER_BASE_MASK) >> RC2_SAT_SATELLITE_DOPPLER_BASE_SHIFT); + if ((iDopplerBase & RC2_SAT_SATELLITE_DOPPLER_BASE_SIGNBIT_MASK) != 0) { iDopplerBase |= RC2_SAT_SATELLITE_DOPPLER_BASE_SIGNEXT_MASK; } - uiRangeDataBytesDecompressed += sizeof(RangeCmp2SatelliteBlockStruct); + uiRangeDataBytesDecompressed += sizeof(RangeCmp2SatelliteBlock); // Decompress the Signal Blocks associated with the Satellite Block - for (uint8_t ucSignalBlockIndex = 0; ucSignalBlockIndex < ucNumberOfSignalBlocks; ucSignalBlockIndex++) + for (uint8_t ucSignalBlockIndex = 0; ucSignalBlockIndex < ucSignalBlockCount; ucSignalBlockIndex++) { - const auto& stRangeCmp2SigBlock = - reinterpret_cast(stRangeCmp2Message_.aucRangeData[uiRangeDataBytesDecompressed]); - - const auto eSignalType = static_cast(stRangeCmp2SigBlock.uiCombinedField1 & RC2_SIG_SIGNAL_TYPE_MASK); - const auto ucPSRBitfield = - static_cast((stRangeCmp2SigBlock.ulCombinedField2 & RC2_SIG_PSR_STDDEV_MASK) >> RC2_SIG_PSR_STDDEV_SHIFT); - const auto ucADRBitfield = - static_cast((stRangeCmp2SigBlock.ulCombinedField2 & RC2_SIG_ADR_STDDEV_MASK) >> RC2_SIG_ADR_STDDEV_SHIFT); - const auto uiLocktimeBits = - static_cast((stRangeCmp2SigBlock.uiCombinedField1 & RC2_SIG_LOCKTIME_MASK) >> RC2_SIG_LOCKTIME_SHIFT); - const auto usPRN = static_cast(stRangeCmp2SatBlock.ucSatelliteIdentifier + - (eSatelliteSystem == SYSTEM::GLONASS ? (GLONASS_SLOT_OFFSET - 1) : 0)); + const auto& stSigBlock = reinterpret_cast(stRangeCmp2Message_.aucRangeData[uiRangeDataBytesDecompressed]); + + const auto eSignalType = static_cast(stSigBlock.uiCombinedField1 & RC2_SIG_SIGNAL_TYPE_MASK); + const auto ucPSRBitfield = static_cast((stSigBlock.ullCombinedField2 & RC2_SIG_PSR_STDDEV_MASK) >> RC2_SIG_PSR_STDDEV_SHIFT); + const auto ucADRBitfield = static_cast((stSigBlock.ullCombinedField2 & RC2_SIG_ADR_STDDEV_MASK) >> RC2_SIG_ADR_STDDEV_SHIFT); + const auto uiLocktimeBits = static_cast((stSigBlock.uiCombinedField1 & RC2_SIG_LOCKTIME_MASK) >> RC2_SIG_LOCKTIME_SHIFT); + const auto usPRN = static_cast(stSatBlock.ucSatelliteIdentifier + (eSystem == SYSTEM::GLONASS ? GLONASS_SLOT_OFFSET - 1 : 0)); // Extend the sign - auto iDopplerBitfield = static_cast(static_cast(stRangeCmp2SigBlock.ulCombinedField2 & RC2_SIG_DOPPLER_DIFF_MASK) >> - RC2_SIG_DOPPLER_DIFF_SHIFT); - if ((iDopplerBitfield & RC2_SIG_DOPPLER_DIFF_SIGNBIT_MASK) != 0U) { iDopplerBitfield |= RC2_SIG_DOPPLER_DIFF_SIGNEXT_MASK; } + auto iDopplerBitfield = static_cast((stSigBlock.ullCombinedField2 & RC2_SIG_DOPPLER_DIFF_MASK) >> RC2_SIG_DOPPLER_DIFF_SHIFT); + if ((iDopplerBitfield & RC2_SIG_DOPPLER_DIFF_SIGNBIT_MASK) != 0) { iDopplerBitfield |= RC2_SIG_DOPPLER_DIFF_SIGNEXT_MASK; } - const auto fPSRDiff = static_cast((stRangeCmp2SigBlock.ulCombinedField2 & RC2_SIG_PSR_DIFF_MASK) >> RC2_SIG_PSR_DIFF_SHIFT); - const auto fPhaseRangeDiff = static_cast( - static_cast(stRangeCmp2SigBlock.ulCombinedField2 & RC2_SIG_PHASERANGE_DIFF_MASK) >> RC2_SIG_PHASERANGE_DIFF_SHIFT); + const auto fPSRDiff = static_cast((stSigBlock.ullCombinedField2 & RC2_SIG_PSR_DIFF_MASK) >> RC2_SIG_PSR_DIFF_SHIFT); + const auto fPhaseRangeDiff = + static_cast((stSigBlock.ullCombinedField2 & RC2_SIG_PHASERANGE_DIFF_MASK) >> RC2_SIG_PHASERANGE_DIFF_SHIFT); const auto fScaledDopplerDiff = static_cast(static_cast(iDopplerBitfield) / RC2_SIG_DOPPLER_DIFF_SCALE_FACTOR); - // Construct the ChannelTrackingStatus word - ChannelTrackingStatusStruct stChannelTrackingStatus(stRangeCmp2SatBlock, stRangeCmp2SigBlock); + ChannelTrackingStatus stChannelTrackingStatus(stSatBlock, stSigBlock); // Construct the decompressed range data - RangeDataStruct& stRangeData = stRangeMessage_.astRangeData[stRangeMessage_.uiNumberOfObservations++]; + RangeData& stRangeData = stRangeMessage_.astRangeData[stRangeMessage_.uiNumberOfObservations++]; stRangeData.usPRN = usPRN; - stRangeData.sGLONASSFrequency = static_cast(stRangeCmp2SatBlock.ulCombinedField & RC2_SAT_GLONASS_FREQUENCY_ID_MASK); + stRangeData.sGLONASSFrequency = static_cast(stSatBlock.ulCombinedField & RC2_SAT_GLONASS_FREQUENCY_ID_MASK); stRangeData.dPSR = static_cast(iPSRBase + static_cast(fPSRDiff / RC2_SIG_PSR_DIFF_SCALE_FACTOR)); - stRangeData.fPSRStdDev = afTheRangeCmp2PSRStdDevValues[ucPSRBitfield]; - stRangeData.dADR = MAGIC_NEGATE * - (static_cast(iPSRBase) + static_cast(fPhaseRangeDiff / RC2_SIG_PHASERANGE_DIFF_SCALE_FACTOR)) / - (GetSignalWavelength(stChannelTrackingStatus, (stRangeData.sGLONASSFrequency - GLONASS_FREQUENCY_NUMBER_OFFSET))); - stRangeData.fADRStdDev = afTheRangeCmp2ADRStdDevValues[ucADRBitfield]; - stRangeData.fDopplerFrequency = static_cast(iDopplerBase + fScaledDopplerDiff) / - static_cast(mmTheRangeCmp2SignalScalingMapping[eSatelliteSystem][eSignalType]); - stRangeData.fCNo = RC2_SIG_CNO_SCALE_OFFSET + static_cast(stRangeCmp2SigBlock.ulCombinedField2 & RC2_SIG_CNO_MASK); - stRangeData.fLockTime = DetermineRangeCmp2ObservationLockTime(stMetaData_, uiLocktimeBits, stChannelTrackingStatus.eSatelliteSystem, - stChannelTrackingStatus.eSignalType, usPRN); + stRangeData.fPSRStdDev = stdDevPsrScaling[ucPSRBitfield]; + stRangeData.dADR = -static_cast(iPSRBase) + + static_cast(fPhaseRangeDiff / RC2_SIG_PHASERANGE_DIFF_SCALE_FACTOR) / + (GetSignalWavelength(stChannelTrackingStatus, (stRangeData.sGLONASSFrequency - GLONASS_FREQUENCY_NUMBER_OFFSET))); + stRangeData.fADRStdDev = stdDevAdrScaling[ucADRBitfield]; + stRangeData.fDopplerFrequency = + static_cast(iDopplerBase + fScaledDopplerDiff) / static_cast(mmTheRangeCmp2SignalScalingMapping[eSystem][eSignalType]); + stRangeData.fCNo = RC2_SIG_CNO_SCALE_OFFSET + static_cast(stSigBlock.ullCombinedField2 & RC2_SIG_CNO_MASK); + stRangeData.fLockTime = GetRangeCmp2LockTime(stMetaData_, uiLocktimeBits, stChannelTrackingStatus.eSatelliteSystem, + stChannelTrackingStatus.eSignalType, usPRN); stRangeData.uiChannelTrackingStatus = stChannelTrackingStatus.GetAsWord(); - uiRangeDataBytesDecompressed += sizeof(RangeCmp2SignalBlockStruct); + uiRangeDataBytesDecompressed += sizeof(RangeCmp2SignalBlock); } } } //------------------------------------------------------------------------------ -//! Decompress a buffer containing a RANGECMP4 message and translate it into -//! a RANGE message. +// Convert a RANGECMP4 message into RANGE message. //------------------------------------------------------------------------------ -void RangeDecompressor::RangeCmp4ToRange(uint8_t* pucCompressedData_, RangeStruct& stRangeMessage_, const MetaDataStruct& stMetaData_) +void RangeDecompressor::RangeCmp4ToRange(unsigned char* pucData_, Range& stRangeMessage_, const MetaDataStruct& stMetaData_) { - uint8_t* pucTempDataPointer = pucCompressedData_; - MEASUREMENT_SOURCE eMeasurementSource = stMetaData_.eMeasurementSource; double dSecondOffset = static_cast(static_cast(stMetaData_.dMilliseconds) % SEC_TO_MILLI_SEC) / SEC_TO_MILLI_SEC; // Clear any dead reference blocks on the whole second. We should be storing new ones. if (dSecondOffset == 0.0) { ammmMyReferenceBlocks[static_cast(eMeasurementSource)].clear(); } - std::vector vSignals; // All available signals - std::vector vPRNs; // All available PRNs - std::map mIncludedSignals; // IncludedSignal bitmasks for each PRN. - - uint16_t usSatelliteSystems = 0; - uint64_t ulSatellites = 0; - uint16_t usSignals = 0; - - uint64_t ulIncludedSignals = 0; - uint32_t uiSignalBitMaskShift = 0; - uint32_t uiSignalsForPRN = 0; - - bool bPrimaryBlock = false; - ChannelTrackingStatusStruct stChannelTrackingStatus; - RangeCmp4MeasurementBlockHeaderStruct stMeasurementBlockHeader; - RangeCmp4MeasurementSignalBlockStruct stMeasurementBlock; + ChannelTrackingStatus stChannelTrackingStatus; + RangeCmp4MeasurementBlockHeader stMeasurementBlockHeader; + RangeCmp4MeasurementSignalBlock stMeasurementBlock; - // Reset bit offset and 0 the observation count. - uiMyBitOffset = 0; stRangeMessage_.uiNumberOfObservations = 0; + uint32_t uiBitOffset = 0; + uint32_t uiBytesLeft = *reinterpret_cast(pucData_); + pucData_ += sizeof(uint32_t); + auto usSatelliteSystems = ExtractBitfield(&pucData_, uiBytesLeft, uiBitOffset, RC4_SATELLITE_SYSTEMS_BITS); - // Pull out the first few fields. - // We have to set uiMyBytesRemaining by typecasting because GetBitfieldFromBuffer() relies on - // it. - uiMyBytesRemaining = *reinterpret_cast(pucTempDataPointer); - pucTempDataPointer += sizeof(uint32_t); - usSatelliteSystems = static_cast(GetBitfieldFromBuffer(&pucTempDataPointer, RC4_SATELLITE_SYSTEMS_BITS)); - - // For each satellite system, we will decode a series of measurement block headers, each with - // their own subsequent reference signal measurement blocks. - for (const auto& aeTheRangeCmp4SatelliteSystem : aeTheRangeCmp4SatelliteSystems) + // Decode measurement block headers and their reference signal blocks for each satellite system. + for (const auto satelliteSystem : {SYSTEM::GPS, SYSTEM::GLONASS, SYSTEM::SBAS, SYSTEM::GALILEO, SYSTEM::BEIDOU, SYSTEM::QZSS, SYSTEM::NAVIC}) { - vSignals.clear(); - vPRNs.clear(); - // Does this message have any data for this satellite system? - if ((usSatelliteSystems & (1UL << static_cast(aeTheRangeCmp4SatelliteSystem))) != 0U) + if ((usSatelliteSystems & (1UL << static_cast(satelliteSystem))) != 0) { - ulSatellites = GetBitfieldFromBuffer(&pucTempDataPointer, RC4_SATELLITES_BITS); - usSignals = static_cast(GetBitfieldFromBuffer(&pucTempDataPointer, RC4_SIGNALS_BITS)); + auto ullSatellites = ExtractBitfield(&pucData_, uiBytesLeft, uiBitOffset, RC4_SATELLITES_BITS); + auto ullSignals = ExtractBitfield(&pucData_, uiBytesLeft, uiBitOffset, RC4_SIGNALS_BITS); + + std::vector vSignals; // All available signals + std::vector vPRNs; // All available PRNs // Collect the signals tracked in this satellite system. - for (RangeCmp4::SIGNAL_TYPE eCurrentSignalType : mvTheRangeCmp4SystemSignalMasks[aeTheRangeCmp4SatelliteSystem]) + for (rangecmp4::SIGNAL_TYPE eCurrentSignalType : mvTheRangeCmp4SystemSignalMasks[satelliteSystem]) { - if ((usSignals & (1UL << static_cast(eCurrentSignalType))) != 0U) { vSignals.push_back(eCurrentSignalType); } + if ((ullSignals & (1ULL << static_cast(eCurrentSignalType))) != 0) { vSignals.push_back(eCurrentSignalType); } } // Collect the satellite PRNs tracked in this satellite system. for (uint8_t ucBitPosition = 0; ucBitPosition < RC4_SATELLITES_BITS; ucBitPosition++) { // Note that ucBitPosition contains the PRN value at this point. - if ((ulSatellites & (1ULL << ucBitPosition)) != 0U) - { - vPRNs.push_back(ucBitPosition + 1); // Bit position is PRN-1, so +1 here - } + if ((ullSatellites & (1ULL << ucBitPosition)) != 0) { vPRNs.push_back(ucBitPosition + 1); } // Bit position is PRN - 1, so + 1 here } - // Iterate through the PRNs once to collect the signals tracked by each. We need this - // info before we can start decompressing. - mIncludedSignals.clear(); + std::map mIncludedSignals; // IncludedSignal bitmasks for each PRN. + // Iterate through the PRNs once to collect the signals tracked by each. We need this info before we can start decompressing. for (const auto& uiPRN : vPRNs) { - // Get the m*n bit matrix that describes the included signals in this RANGECMP4 - // message. - mIncludedSignals[uiPRN] = GetBitfieldFromBuffer(&pucTempDataPointer, static_cast(vSignals.size())); + // Get the m*n bit matrix that describes the included signals in this RANGECMP4 message. + mIncludedSignals[uiPRN] = ExtractBitfield(&pucData_, uiBytesLeft, uiBitOffset, static_cast(vSignals.size())); } - // Check each PRN against the signals tracked in this satellite system to see if the - // signal is included. + // Check each PRN against the signals tracked in this satellite system to see if the signal is included. for (const auto& uiPRN : vPRNs) { // Begin decoding Reference Measurement Block Header. stMeasurementBlockHeader.bIsDifferentialData = - static_cast(GetBitfieldFromBuffer(&pucTempDataPointer, RC4_MBLK_HDR_DATAFORMAT_FLAG_BITS)); + ExtractBitfield(&pucData_, uiBytesLeft, uiBitOffset, RC4_MBLK_HDR_DATAFORMAT_FLAG_BITS); stMeasurementBlockHeader.ucReferenceDataBlockID = - static_cast(GetBitfieldFromBuffer(&pucTempDataPointer, RC4_MBLK_HDR_REFERENCE_DATABLOCK_ID_BITS)); + ExtractBitfield(&pucData_, uiBytesLeft, uiBitOffset, RC4_MBLK_HDR_REFERENCE_DATABLOCK_ID_BITS); stMeasurementBlockHeader.cGLONASSFrequencyNumber = 0; // This field is only present for GLONASS and reference blocks. - if (aeTheRangeCmp4SatelliteSystem == SYSTEM::GLONASS && !stMeasurementBlockHeader.bIsDifferentialData) + if (satelliteSystem == SYSTEM::GLONASS && !stMeasurementBlockHeader.bIsDifferentialData) { stMeasurementBlockHeader.cGLONASSFrequencyNumber = - static_cast(GetBitfieldFromBuffer(&pucTempDataPointer, RC4_MBLK_HDR_GLONASS_FREQUENCY_NUMBER_BITS)); + ExtractBitfield(&pucData_, uiBytesLeft, uiBitOffset, RC4_MBLK_HDR_GLONASS_FREQUENCY_NUMBER_BITS); } - uiSignalsForPRN = 0; - uiSignalBitMaskShift = 0; - bPrimaryBlock = true; // Reset to true for the first reference block from each PRN. - for (RangeCmp4::SIGNAL_TYPE eCurrentSignalType : vSignals) - { - // Get the included signals bitmask for this PRN. - try - { - ulIncludedSignals = mIncludedSignals.at(uiPRN); - } - catch (...) - { - pclMyLogger->critical("No included signal bitmask for the PRN {}.", uiPRN); - return; - } + uint32_t uiSignalsForPRN = 0; + uint32_t uiSignalBitMaskShift = 0; + bool bPrimaryBlock = true; - // Is the signal included? - if ((ulIncludedSignals & (1ULL << uiSignalBitMaskShift++)) != 0U) + for (rangecmp4::SIGNAL_TYPE eCurrentSignalType : vSignals) + { + if (auto it = mIncludedSignals.find(uiPRN); it != mIncludedSignals.end()) { - if (!stMeasurementBlockHeader.bIsDifferentialData) // This is a reference block. + // Is the signal included? + if ((it->second & (1ULL << uiSignalBitMaskShift++)) != 0) { - if (bPrimaryBlock) + if (!stMeasurementBlockHeader.bIsDifferentialData) // This is a reference block. { - DecompressReferenceBlock(&pucTempDataPointer, stMeasurementBlock, eMeasurementSource); - bPrimaryBlock = false; - } - else { DecompressReferenceBlock(&pucTempDataPointer, stMeasurementBlock, eMeasurementSource); } + if (bPrimaryBlock) + { + DecompressReferenceBlock(&pucData_, uiBytesLeft, uiBitOffset, stMeasurementBlock, eMeasurementSource); + bPrimaryBlock = false; + } + else { DecompressReferenceBlock(&pucData_, uiBytesLeft, uiBitOffset, stMeasurementBlock, eMeasurementSource); } - stChannelTrackingStatus = - ChannelTrackingStatusStruct(aeTheRangeCmp4SatelliteSystem, eCurrentSignalType, stMeasurementBlock); - PopulateNextRangeData((stRangeMessage_.astRangeData[stRangeMessage_.uiNumberOfObservations++]), stMeasurementBlock, - stMetaData_, stChannelTrackingStatus, uiPRN, stMeasurementBlockHeader.cGLONASSFrequencyNumber); + stChannelTrackingStatus = ChannelTrackingStatus(satelliteSystem, eCurrentSignalType, stMeasurementBlock); + PopulateNextRangeData((stRangeMessage_.astRangeData[stRangeMessage_.uiNumberOfObservations++]), stMeasurementBlock, + stMetaData_, stChannelTrackingStatus, uiPRN, stMeasurementBlockHeader.cGLONASSFrequencyNumber); - // Always store reference blocks. - ammmMyReferenceBlocks[static_cast(eMeasurementSource)][aeTheRangeCmp4SatelliteSystem][eCurrentSignalType] - [uiPRN] = std::pair(stMeasurementBlockHeader, stMeasurementBlock); - } - else // This is a differential block. - { - RangeCmp4MeasurementBlockHeaderStruct* pstReferenceBlockHeader = nullptr; - RangeCmp4MeasurementSignalBlockStruct* pstReferenceBlock = nullptr; - try - { - pstReferenceBlockHeader = &ammmMyReferenceBlocks[static_cast(eMeasurementSource)] - .at(aeTheRangeCmp4SatelliteSystem) - .at(eCurrentSignalType) - .at(uiPRN) - .first; - pstReferenceBlock = &ammmMyReferenceBlocks[static_cast(eMeasurementSource)] - .at(aeTheRangeCmp4SatelliteSystem) - .at(eCurrentSignalType) - .at(uiPRN) - .second; + // Always store reference blocks. + ammmMyReferenceBlocks[static_cast(eMeasurementSource)][satelliteSystem][eCurrentSignalType][uiPRN] = + std::pair(stMeasurementBlockHeader, stMeasurementBlock); } - catch (...) + else // This is a differential block. { - pclMyLogger->warn("No reference data exists for SATELLITE_SYSTEM {}, SIGNAL_TYPE " - "{}, PRN {}, ID {}", - static_cast(aeTheRangeCmp4SatelliteSystem), static_cast(eCurrentSignalType), - uiPRN, stMeasurementBlockHeader.ucReferenceDataBlockID); - } + RangeCmp4MeasurementBlockHeader* pstRefBlockHeader = nullptr; + RangeCmp4MeasurementSignalBlock* pstRefBlock = nullptr; + try + { + pstRefBlockHeader = &ammmMyReferenceBlocks[static_cast(eMeasurementSource)] + .at(satelliteSystem) + .at(eCurrentSignalType) + .at(uiPRN) + .first; + pstRefBlock = &ammmMyReferenceBlocks[static_cast(eMeasurementSource)] + .at(satelliteSystem) + .at(eCurrentSignalType) + .at(uiPRN) + .second; + } + catch (...) + { + pclMyLogger->warn("No reference data exists for SATELLITE_SYSTEM {}, SIGNAL_TYPE {}, PRN {}, ID {}", + static_cast(satelliteSystem), static_cast(eCurrentSignalType), uiPRN, + stMeasurementBlockHeader.ucReferenceDataBlockID); + } - // Do nothing if we can't find reference data. - if (pstReferenceBlockHeader != nullptr && pstReferenceBlock != nullptr) - { - if (stMeasurementBlockHeader.ucReferenceDataBlockID == pstReferenceBlockHeader->ucReferenceDataBlockID) + // Do nothing if we can't find reference data. + if (pstRefBlockHeader != nullptr && pstRefBlock != nullptr) { - if (bPrimaryBlock) + if (stMeasurementBlockHeader.ucReferenceDataBlockID == pstRefBlockHeader->ucReferenceDataBlockID) { - DecompressDifferentialBlock(&pucTempDataPointer, stMeasurementBlock, *pstReferenceBlock, - dSecondOffset); - bPrimaryBlock = false; + if (bPrimaryBlock) + { + DecompressDifferentialBlock(&pucData_, uiBytesLeft, uiBitOffset, stMeasurementBlock, *pstRefBlock, + dSecondOffset); + bPrimaryBlock = false; + } + else + { + DecompressDifferentialBlock(&pucData_, uiBytesLeft, uiBitOffset, stMeasurementBlock, *pstRefBlock, + dSecondOffset); + } + + stChannelTrackingStatus = ChannelTrackingStatus(satelliteSystem, eCurrentSignalType, stMeasurementBlock); + PopulateNextRangeData(stRangeMessage_.astRangeData[stRangeMessage_.uiNumberOfObservations++], + stMeasurementBlock, stMetaData_, stChannelTrackingStatus, uiPRN, + pstRefBlockHeader->cGLONASSFrequencyNumber); } else { - DecompressDifferentialBlock(&pucTempDataPointer, stMeasurementBlock, *pstReferenceBlock, dSecondOffset); + pclMyLogger->warn("Invalid reference data: Diff ID {} != Ref ID {}", + stMeasurementBlockHeader.ucReferenceDataBlockID, pstRefBlockHeader->ucReferenceDataBlockID); } - - stChannelTrackingStatus = - ChannelTrackingStatusStruct(aeTheRangeCmp4SatelliteSystem, eCurrentSignalType, stMeasurementBlock); - PopulateNextRangeData(stRangeMessage_.astRangeData[stRangeMessage_.uiNumberOfObservations++], stMeasurementBlock, - stMetaData_, stChannelTrackingStatus, uiPRN, - pstReferenceBlockHeader->cGLONASSFrequencyNumber); - } - else - { - pclMyLogger->warn("Invalid reference data: Diff ID {} != Ref ID {}", - static_cast(stMeasurementBlockHeader.ucReferenceDataBlockID), - static_cast(pstReferenceBlockHeader->ucReferenceDataBlockID)); } } - } - // Keep track of how many signals came from this PRN so we can go back and - // alter the message fields we cannot know until some time in the future. - uiSignalsForPRN++; + // Track the number of signals from this PRN to update unknown message fields later. + uiSignalsForPRN++; + } + } + else + { + pclMyLogger->critical("No included signal bitmask for the PRN {}.", uiPRN); + return; } } - // Go back and update the grouping bit in the status word if we've counted more than - // one signal for this PRN. We can't know this ahead of time without blindly - // jumping forward into the compressed message. + // Update the grouping bit in the status word if multiple signals for this PRN are counted. if (uiSignalsForPRN > 1 && uiSignalsForPRN <= stRangeMessage_.uiNumberOfObservations) { for (uint32_t uiIndex = uiSignalsForPRN; uiIndex > 0; uiIndex--) @@ -979,15 +879,12 @@ void RangeDecompressor::RangeCmp4ToRange(uint8_t* pucCompressedData_, RangeStruc } //------------------------------------------------------------------------------ -//! This method will decompress the range message provided in pucRangeMessageBuffer_ +//! This method will decompress the range message provided in pucBuffer_ //! and overwrite the contents with the decompressed message. //------------------------------------------------------------------------------ -STATUS -RangeDecompressor::Decompress(unsigned char* pucRangeMessageBuffer_, uint32_t uiRangeMessageBufferSize_, MetaDataStruct& stMetaData_, - ENCODE_FORMAT eFormat_) +STATUS RangeDecompressor::Decompress(unsigned char* pucBuffer_, uint32_t uiBufferSize_, MetaDataStruct& stMetaData_, ENCODE_FORMAT eFormat_) { - // Check for buffer validity - if (pucRangeMessageBuffer_ == nullptr) { return STATUS::NULL_PROVIDED; } + if (pucBuffer_ == nullptr) { return STATUS::NULL_PROVIDED; } if (pclMyMsgDB == nullptr) { return STATUS::NO_DATABASE; } @@ -996,24 +893,21 @@ RangeDecompressor::Decompress(unsigned char* pucRangeMessageBuffer_, uint32_t ui std::vector stMessage; auto eStatus = STATUS::UNKNOWN; - unsigned char* pucTempMessagePointer = pucRangeMessageBuffer_; + unsigned char* pucTempMessagePointer = pucBuffer_; eStatus = clMyHeaderDecoder.Decode(pucTempMessagePointer, stHeader, stMetaData_); if (eStatus != STATUS::SUCCESS) { return eStatus; } if (!clMyRangeCmpFilter.DoFiltering(stMetaData_)) { return STATUS::UNSUPPORTED; } - HEADER_FORMAT eInitialFormat = stMetaData_.eFormat; + HEADER_FORMAT eFormat = stMetaData_.eFormat; pucTempMessagePointer += stMetaData_.uiHeaderLength; - // If the message is already in binary format, we don't need to do anything. - // If the message is not in binary format, we need to ensure that it is encoded to binary so - // that it can be decompressed. - if (eInitialFormat != HEADER_FORMAT::BINARY) + // If the message is not in binary format, we need to ensure that it is encoded to binary so that it can be decompressed. + if (eFormat != HEADER_FORMAT::BINARY) { eStatus = clMyMessageDecoder.Decode(pucTempMessagePointer, stMessage, stMetaData_); if (eStatus != STATUS::SUCCESS) { return eStatus; } - eStatus = clMyEncoder.Encode(&pucRangeMessageBuffer_, uiRangeMessageBufferSize_, stHeader, stMessage, stMessageData, stMetaData_, - ENCODE_FORMAT::FLATTENED_BINARY); + eStatus = clMyEncoder.Encode(&pucBuffer_, uiBufferSize_, stHeader, stMessage, stMessageData, stMetaData_, ENCODE_FORMAT::FLATTENED_BINARY); if (eStatus != STATUS::SUCCESS) { return eStatus; } pucTempMessagePointer = stMessageData.pucMessageBody; @@ -1022,18 +916,18 @@ RangeDecompressor::Decompress(unsigned char* pucRangeMessageBuffer_, uint32_t ui // Convert the RANGECMPx message to a RANGE message try { - RangeStruct stRange; + Range stRange; switch (stMetaData_.usMessageId) { - case RANGECMP_MSG_ID: RangeCmpToRange(*reinterpret_cast(pucTempMessagePointer), stRange); break; - case RANGECMP2_MSG_ID: RangeCmp2ToRange(*reinterpret_cast(pucTempMessagePointer), stRange, stMetaData_); break; + case RANGECMP_MSG_ID: RangeCmpToRange(*reinterpret_cast(pucTempMessagePointer), stRange); break; + case RANGECMP2_MSG_ID: RangeCmp2ToRange(*reinterpret_cast(pucTempMessagePointer), stRange, stMetaData_); break; case RANGECMP3_MSG_ID: [[fallthrough]]; case RANGECMP4_MSG_ID: RangeCmp4ToRange(pucTempMessagePointer, stRange, stMetaData_); break; default: return STATUS::UNSUPPORTED; } // Set the binary message length in the metadata for decoding purposes. - stMetaData_.uiBinaryMsgLength = sizeof(stRange.uiNumberOfObservations) + (stRange.uiNumberOfObservations * sizeof(RangeDataStruct)); + stMetaData_.uiBinaryMsgLength = sizeof(stRange.uiNumberOfObservations) + stRange.uiNumberOfObservations * sizeof(RangeData); memcpy(pucTempMessagePointer, &stRange, stMetaData_.uiBinaryMsgLength); } catch (...) @@ -1053,22 +947,20 @@ RangeDecompressor::Decompress(unsigned char* pucRangeMessageBuffer_, uint32_t ui eStatus = clMyMessageDecoder.Decode(pucTempMessagePointer, stMessage, stMetaData_); if (eStatus != STATUS::SUCCESS) { return eStatus; } - stMetaData_.eFormat = eInitialFormat; + stMetaData_.eFormat = eFormat; // Re-encode to the original format if a format was not specified. if (eFormat_ == ENCODE_FORMAT::UNSPECIFIED) { - eFormat_ = - (eInitialFormat == HEADER_FORMAT::BINARY || eInitialFormat == HEADER_FORMAT::SHORT_BINARY || - eInitialFormat == HEADER_FORMAT::PROPRIETARY_BINARY) - ? ENCODE_FORMAT::BINARY - : (eInitialFormat == HEADER_FORMAT::ASCII || eInitialFormat == HEADER_FORMAT::SHORT_ASCII || eInitialFormat == HEADER_FORMAT::ABB_ASCII) - ? ENCODE_FORMAT::ASCII - : (eInitialFormat == HEADER_FORMAT::JSON) ? ENCODE_FORMAT::JSON - : ENCODE_FORMAT::ASCII; // Default to ASCII + eFormat_ = eFormat == HEADER_FORMAT::BINARY || eFormat == HEADER_FORMAT::SHORT_BINARY || eFormat == HEADER_FORMAT::PROPRIETARY_BINARY + ? ENCODE_FORMAT::BINARY + : eFormat == HEADER_FORMAT::ASCII || eFormat == HEADER_FORMAT::SHORT_ASCII || eFormat == HEADER_FORMAT::ABB_ASCII + ? ENCODE_FORMAT::ASCII + : eFormat == HEADER_FORMAT::JSON ? ENCODE_FORMAT::JSON + : ENCODE_FORMAT::ASCII; // Default to ASCII } // Re-encode the data back into the range message buffer. - eStatus = clMyEncoder.Encode(&pucRangeMessageBuffer_, uiRangeMessageBufferSize_, stHeader, stMessage, stMessageData, stMetaData_, eFormat_); + eStatus = clMyEncoder.Encode(&pucBuffer_, uiBufferSize_, stHeader, stMessage, stMessageData, stMetaData_, eFormat_); if (eStatus != STATUS::SUCCESS) { return eStatus; } // Final adjustments to MetaData diff --git a/src/decoders/oem/test/range_cmp_test.cpp b/src/decoders/oem/test/range_cmp_test.cpp index 20a3c7c7e..71c7b8b4e 100644 --- a/src/decoders/oem/test/range_cmp_test.cpp +++ b/src/decoders/oem/test/range_cmp_test.cpp @@ -39,16 +39,10 @@ class RangeCmpTest : public ::testing::Test RangeDecompressorTester(JsonReader* pclJsonDb_) : RangeDecompressor(pclJsonDb_) {} // Access protected member of RangeDecompressor - void SetBitOffset(uint32_t uiBitOffset_) { uiMyBitOffset = uiBitOffset_; } - - // Access protected member of RangeDecompressor - void SetBytesRemaining(uint32_t uiByteCount_) { uiMyBytesRemaining = uiByteCount_; } - - // Access protected member of RangeDecompressor - uint32_t GetBytesRemaining() { return uiMyBytesRemaining; } - - // Access protected member of RangeDecompressor - uint64_t GetBitfield(uint8_t** ppucBytes_, uint32_t uiBitfieldSize_) { return GetBitfieldFromBuffer(ppucBytes_, uiBitfieldSize_); } + uint64_t GetBitfield(uint8_t** ppucBytes_, uint32_t& uiBytesLeft_, uint32_t& uiBitOffset_, uint32_t uiBitfieldSize_) + { + return ExtractBitfield(ppucBytes_, uiBytesLeft_, uiBitOffset_, uiBitfieldSize_); + } void ResetLockTimes() { @@ -80,7 +74,6 @@ class RangeCmpTest : public ::testing::Test std::unique_ptr RangeCmpTest::pclMyRangeDecompressor = nullptr; std::unique_ptr RangeCmpTest::pclMyJsonDb = nullptr; -// TODO: we disable clang-format because of the long strings // clang-format off // ------------------------------------------------------------------------------------------------------- @@ -102,19 +95,19 @@ TEST_F(RangeCmpTest, LOGGER) TEST_F(RangeCmpTest, CHANNEL_TRACKING_STATUS_WORD_1) { constexpr uint32_t uiCTS = 0x1810BC04; - ASSERT_EQ(uiCTS, ChannelTrackingStatusStruct(uiCTS).GetAsWord()); + ASSERT_EQ(uiCTS, ChannelTrackingStatus(uiCTS).GetAsWord()); } TEST_F(RangeCmpTest, CHANNEL_TRACKING_STATUS_WORD_2) { constexpr uint32_t uiCTS = 0x69129D54; - ASSERT_EQ(uiCTS, ChannelTrackingStatusStruct(uiCTS).GetAsWord()); + ASSERT_EQ(uiCTS, ChannelTrackingStatus(uiCTS).GetAsWord()); } TEST_F(RangeCmpTest, CHANNEL_TRACKING_STATUS_WORD_3) { constexpr uint32_t uiCTS = 0xFBF7FFFF; - ASSERT_EQ(uiCTS, ChannelTrackingStatusStruct(uiCTS).GetAsWord()); + ASSERT_EQ(uiCTS, ChannelTrackingStatus(uiCTS).GetAsWord()); } // ------------------------------------------------------------------------------------------------------- @@ -130,12 +123,11 @@ TEST_F(RangeCmpTest, BITFIELD_1) // Expected bitfield 3: 1000 1... .... .... .... .... (0x11) uint8_t aucBytes[] = {0xDF, 0x76, 0x88}; uint8_t* pucBytesPointer = &aucBytes[0]; // GetBitfield() will advance this pointer. - - pclMyRangeDecompressor->SetBitOffset(0); - pclMyRangeDecompressor->SetBytesRemaining(sizeof(aucBytes)); - ASSERT_EQ(0x76DF, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, 15)); - ASSERT_EQ(0x0, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, 4)); - ASSERT_EQ(0x11, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, 5)); + uint32_t uiBytesLeft = sizeof(aucBytes); + uint32_t uiBitOffset = 0; + ASSERT_EQ(0x76DF, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, uiBytesLeft, uiBitOffset, 15)); + ASSERT_EQ(0x0, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, uiBytesLeft, uiBitOffset, 4)); + ASSERT_EQ(0x11, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, uiBytesLeft, uiBitOffset, 5)); } TEST_F(RangeCmpTest, BITFIELD_2) @@ -146,10 +138,9 @@ TEST_F(RangeCmpTest, BITFIELD_2) // Expected bitfield: XXXX XXX^ ^^^^ ^^^^ ^^^^ ^^^^ ^^^^ ^^^^ ^^^^ ^^^^ ^^^^ .... (0xA07CD81F6) uint8_t aucBytes[] = {0x61, 0x1F, 0xD8, 0x7C, 0xA0, 0xB0}; uint8_t* pucBytesPointer = &aucBytes[0]; // GetBitfield() will advance this pointer. - - pclMyRangeDecompressor->SetBitOffset(4); - pclMyRangeDecompressor->SetBytesRemaining(sizeof(aucBytes)); - ASSERT_EQ(0xA07CD81F6ULL, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, 37)); + uint32_t uiBitOffset = 4; + uint32_t uiBytesLeft = sizeof(aucBytes); + ASSERT_EQ(0xA07CD81F6ULL, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, uiBytesLeft, uiBitOffset, 37)); } TEST_F(RangeCmpTest, BITFIELD_3) @@ -157,10 +148,9 @@ TEST_F(RangeCmpTest, BITFIELD_3) // 9 bytes, 72 bits. We should not be able to ask for more than 64 bits from this function. uint8_t aucBytes[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; uint8_t* pucBytesPointer = &aucBytes[0]; // GetBitfield() will advance this pointer. - - pclMyRangeDecompressor->SetBitOffset(0); - pclMyRangeDecompressor->SetBytesRemaining(sizeof(aucBytes)); - ASSERT_EQ(0x0, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, 65)); + uint32_t uiBitOffset = 0; + uint32_t uiBytesLeft = sizeof(aucBytes); + ASSERT_EQ(0x0, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, uiBytesLeft, uiBitOffset, 65)); } TEST_F(RangeCmpTest, BITFIELD_4) @@ -186,42 +176,42 @@ TEST_F(RangeCmpTest, BITFIELD_4) // Expected bitfield 16: 1XXX XXXX XXXX XXXX (0x1) uint8_t aucBytes[] = {0xAA, 0xAA}; uint8_t* pucBytesPointer = &aucBytes[0]; // GetBitfield() will advance this pointer. - - pclMyRangeDecompressor->SetBitOffset(0); - pclMyRangeDecompressor->SetBytesRemaining(sizeof(aucBytes)); - ASSERT_EQ(0x0, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, 1)); - ASSERT_EQ(2U, pclMyRangeDecompressor->GetBytesRemaining()); - ASSERT_EQ(0x1, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, 1)); - ASSERT_EQ(2U, pclMyRangeDecompressor->GetBytesRemaining()); - ASSERT_EQ(0x0, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, 1)); - ASSERT_EQ(2U, pclMyRangeDecompressor->GetBytesRemaining()); - ASSERT_EQ(0x1, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, 1)); - ASSERT_EQ(2U, pclMyRangeDecompressor->GetBytesRemaining()); - ASSERT_EQ(0x0, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, 1)); - ASSERT_EQ(2U, pclMyRangeDecompressor->GetBytesRemaining()); - ASSERT_EQ(0x1, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, 1)); - ASSERT_EQ(2U, pclMyRangeDecompressor->GetBytesRemaining()); - ASSERT_EQ(0x0, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, 1)); - ASSERT_EQ(2U, pclMyRangeDecompressor->GetBytesRemaining()); - ASSERT_EQ(0x1, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, 1)); - ASSERT_EQ(1U, pclMyRangeDecompressor->GetBytesRemaining()); - - ASSERT_EQ(0x0, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, 1)); - ASSERT_EQ(1U, pclMyRangeDecompressor->GetBytesRemaining()); - ASSERT_EQ(0x1, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, 1)); - ASSERT_EQ(1U, pclMyRangeDecompressor->GetBytesRemaining()); - ASSERT_EQ(0x0, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, 1)); - ASSERT_EQ(1U, pclMyRangeDecompressor->GetBytesRemaining()); - ASSERT_EQ(0x1, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, 1)); - ASSERT_EQ(1U, pclMyRangeDecompressor->GetBytesRemaining()); - ASSERT_EQ(0x0, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, 1)); - ASSERT_EQ(1U, pclMyRangeDecompressor->GetBytesRemaining()); - ASSERT_EQ(0x1, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, 1)); - ASSERT_EQ(1U, pclMyRangeDecompressor->GetBytesRemaining()); - ASSERT_EQ(0x0, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, 1)); - ASSERT_EQ(1U, pclMyRangeDecompressor->GetBytesRemaining()); - ASSERT_EQ(0x1, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, 1)); - ASSERT_EQ(0U, pclMyRangeDecompressor->GetBytesRemaining()); + uint32_t uiBytesLeft = sizeof(aucBytes); + uint32_t uiBitOffset = 0; + + ASSERT_EQ(0x0, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, uiBytesLeft, uiBitOffset, 1)); + ASSERT_EQ(2U, uiBytesLeft); + ASSERT_EQ(0x1, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, uiBytesLeft, uiBitOffset, 1)); + ASSERT_EQ(2U, uiBytesLeft); + ASSERT_EQ(0x0, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, uiBytesLeft, uiBitOffset, 1)); + ASSERT_EQ(2U, uiBytesLeft); + ASSERT_EQ(0x1, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, uiBytesLeft, uiBitOffset, 1)); + ASSERT_EQ(2U, uiBytesLeft); + ASSERT_EQ(0x0, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, uiBytesLeft, uiBitOffset, 1)); + ASSERT_EQ(2U, uiBytesLeft); + ASSERT_EQ(0x1, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, uiBytesLeft, uiBitOffset, 1)); + ASSERT_EQ(2U, uiBytesLeft); + ASSERT_EQ(0x0, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, uiBytesLeft, uiBitOffset, 1)); + ASSERT_EQ(2U, uiBytesLeft); + ASSERT_EQ(0x1, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, uiBytesLeft, uiBitOffset, 1)); + ASSERT_EQ(1U, uiBytesLeft); + + ASSERT_EQ(0x0, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, uiBytesLeft, uiBitOffset, 1)); + ASSERT_EQ(1U, uiBytesLeft); + ASSERT_EQ(0x1, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, uiBytesLeft, uiBitOffset, 1)); + ASSERT_EQ(1U, uiBytesLeft); + ASSERT_EQ(0x0, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, uiBytesLeft, uiBitOffset, 1)); + ASSERT_EQ(1U, uiBytesLeft); + ASSERT_EQ(0x1, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, uiBytesLeft, uiBitOffset, 1)); + ASSERT_EQ(1U, uiBytesLeft); + ASSERT_EQ(0x0, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, uiBytesLeft, uiBitOffset, 1)); + ASSERT_EQ(1U, uiBytesLeft); + ASSERT_EQ(0x1, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, uiBytesLeft, uiBitOffset, 1)); + ASSERT_EQ(1U, uiBytesLeft); + ASSERT_EQ(0x0, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, uiBytesLeft, uiBitOffset, 1)); + ASSERT_EQ(1U, uiBytesLeft); + ASSERT_EQ(0x1, pclMyRangeDecompressor->GetBitfield(&pucBytesPointer, uiBytesLeft, uiBitOffset, 1)); + ASSERT_EQ(0U, uiBytesLeft); } // ------------------------------------------------------------------------------------------------------- From 95c49d48f500422fac16e4bf0c0cb8c90438cafa Mon Sep 17 00:00:00 2001 From: Jonathan McDermid Date: Fri, 25 Oct 2024 20:30:56 -0600 Subject: [PATCH 2/6] remove shift variables, infer value instead --- .../decoders/oem/rangecmp/common.hpp | 122 ++++++++---------- .../oem/src/rangecmp/range_decompressor.cpp | 47 +++---- 2 files changed, 72 insertions(+), 97 deletions(-) diff --git a/include/novatel_edie/decoders/oem/rangecmp/common.hpp b/include/novatel_edie/decoders/oem/rangecmp/common.hpp index 07270e563..d20274883 100644 --- a/include/novatel_edie/decoders/oem/rangecmp/common.hpp +++ b/include/novatel_edie/decoders/oem/rangecmp/common.hpp @@ -130,43 +130,29 @@ constexpr double WAVELENGTH_QZSS_L6 = SPEED_OF_LIGHT / FREQUENCY_HZ_QZSS_L6; constexpr double WAVELENGTH_NAVIC_L5 = SPEED_OF_LIGHT / FREQUENCY_HZ_GPS_L5; //----------------------------------------------------------------------- -//! RANGE Channel Tracking Status data field bit masks and shifts. -//! NOTE: These masks, shifts and offsets will be used to construct or +//! RANGE Channel Tracking Status data field bit masks. +//! NOTE: These masks and offsets will be used to construct or //! deconstruct ChannelTrackingStatus words which appear in RANGE //! messages. //! "CTS" == "Channel Tracking Status" //----------------------------------------------------------------------- constexpr uint32_t CTS_TRACKING_STATE_MASK = 0x0000001F; -constexpr uint32_t CTS_TRACKING_STATE_SHIFT = 0; constexpr uint32_t CTS_SV_CHANNEL_NUMBER_MASK = 0x000003E0; -constexpr uint32_t CTS_SV_CHANNEL_NUMBER_SHIFT = 5; constexpr uint32_t CTS_PHASE_LOCK_MASK = 0x00000400; -constexpr uint32_t CTS_PHASE_LOCK_SHIFT = 10; constexpr uint32_t CTS_PARITY_KNOWN_MASK = 0x00000800; -constexpr uint32_t CTS_PARITY_KNOWN_SHIFT = 11; constexpr uint32_t CTS_CODE_LOCKED_MASK = 0x00001000; -constexpr uint32_t CTS_CODE_LOCKED_SHIFT = 12; constexpr uint32_t CTS_CORRELATOR_MASK = 0x0000E000; -constexpr uint32_t CTS_CORRELATOR_SHIFT = 13; constexpr uint32_t CTS_SATELLITE_SYSTEM_MASK = 0x00070000; -constexpr uint32_t CTS_SATELLITE_SYSTEM_SHIFT = 16; constexpr uint32_t CTS_GROUPING_MASK = 0x00100000; -constexpr uint32_t CTS_GROUPING_SHIFT = 20; constexpr uint32_t CTS_SIGNAL_TYPE_MASK = 0x03E00000; -constexpr uint32_t CTS_SIGNAL_TYPE_SHIFT = 21; constexpr uint32_t CTS_PRIMARY_L1_CHANNEL_MASK = 0x08000000; -constexpr uint32_t CTS_PRIMARY_L1_CHANNEL_SHIFT = 27; constexpr uint32_t CTS_CARRIER_PHASE_MASK = 0x10000000; -constexpr uint32_t CTS_CARRIER_PHASE_SHIFT = 28; constexpr uint32_t CTS_DIGITAL_FILTERING_MASK = 0x20000000; -constexpr uint32_t CTS_DIGITAL_FILTERING_SHIFT = 29; constexpr uint32_t CTS_PRN_LOCK_MASK = 0x40000000; -constexpr uint32_t CTS_PRN_LOCK_SHIFT = 30; constexpr uint32_t CTS_CHANNEL_ASSIGNMENT_MASK = 0x80000000; -constexpr uint32_t CTS_CHANNEL_ASSIGNMENT_SHIFT = 31; //----------------------------------------------------------------------- -//! RANGECMP data field masks, shifts and scale factors. +//! RANGECMP data field masks and scale factors. //! NOTE: RangeCmpData defines a number of fields that can be //! masked out from larger data types. //----------------------------------------------------------------------- @@ -175,24 +161,20 @@ constexpr uint32_t RC_DOPPLER_FREQUENCY_SIGNBIT_MASK = 0x08000000; constexpr uint32_t RC_DOPPLER_FREQUENCY_SIGNEXT_MASK = 0xF0000000; constexpr float RC_DOPPLER_FREQUENCY_SCALE_FACTOR = 256.0F; constexpr uint64_t RC_PSR_MEASUREMENT_MASK = 0xFFFFFFFFF0000000; -constexpr uint32_t RC_PSR_MEASUREMENT_SHIFT = 28; constexpr double RC_PSR_MEASUREMENT_SCALE_FACTOR = 128.0; constexpr double RC_ADR_SCALE_FACTOR = 256.0; constexpr uint32_t RC_PSR_STDDEV_MASK = 0x0F; constexpr uint32_t RC_ADR_STDDEV_MASK = 0xF0; -constexpr uint32_t RC_ADR_STDDEV_SHIFT = 4; constexpr double RC_ADR_STDDEV_SCALE_FACTOR = 512.0; constexpr uint32_t RC_ADR_STDDEV_SCALE_OFFSET = 1; constexpr uint32_t RC_LOCK_TIME_MASK = 0x001FFFFF; constexpr double RC_LOCK_TIME_SCALE_FACTOR = 32.0; constexpr uint32_t RC_CNO_MASK = 0x03E00000; -constexpr uint32_t RC_CNO_SHIFT = 21; constexpr uint32_t RC_CNO_SCALE_OFFSET = 20; constexpr uint32_t RC_GLONASS_FREQUENCY_MASK = 0xFC000000; -constexpr uint32_t RC_GLONASS_FREQUENCY_SHIFT = 26; //----------------------------------------------------------------------- -//! RANGECMP2 data field masks, shifts and scale factors. +//! RANGECMP2 data field masks and scale factors. //! NOTE: RANGECMP2 contains two kinds of blocks - Satellite (SAT) and //! Signal (SIG) - each with oddly sized bitfields. The bitfields can be //! masked out of larger fields (that are multiples of bytes) which @@ -204,49 +186,33 @@ constexpr uint32_t RC_GLONASS_FREQUENCY_SHIFT = 26; //----------------------------------------------------------------------- constexpr uint64_t RC2_SAT_GLONASS_FREQUENCY_ID_MASK = 0x000000000000000F; constexpr uint64_t RC2_SAT_SATELLITE_SYSTEM_ID_MASK = 0x00000000000001F0; -constexpr uint32_t RC2_SAT_SATELLITE_SYSTEM_ID_SHIFT = 4; constexpr uint64_t RC2_SAT_SATELLITE_PSR_BASE_MASK = 0x0000007FFFFFFC00; -constexpr uint32_t RC2_SAT_SATELLITE_PSR_BASE_SHIFT = 10; constexpr uint32_t RC2_SAT_SATELLITE_PSR_BASE_SIGNBIT_MASK = 0x10000000; constexpr uint32_t RC2_SAT_SATELLITE_PSR_BASE_SIGNEXT_MASK = 0xE0000000; constexpr uint64_t RC2_SAT_SATELLITE_DOPPLER_BASE_MASK = 0x0FFFFF8000000000; -constexpr uint32_t RC2_SAT_SATELLITE_DOPPLER_BASE_SHIFT = 39; constexpr uint32_t RC2_SAT_SATELLITE_DOPPLER_BASE_SIGNBIT_MASK = 0x00100000; constexpr uint32_t RC2_SAT_SATELLITE_DOPPLER_BASE_SIGNEXT_MASK = 0xFFE00000; constexpr uint64_t RC2_SAT_NUM_SIGNAL_BLOCKS_BASE_MASK = 0xF000000000000000; -constexpr uint32_t RC2_SAT_NUM_SIGNAL_BLOCKS_BASE_SHIFT = 60; // For combined field uiCombinedField1 constexpr uint32_t RC2_SIG_SIGNAL_TYPE_MASK = 0x0000001F; constexpr uint32_t RC2_SIG_PHASE_LOCK_MASK = 0x00000020; -constexpr uint32_t RC2_SIG_PHASE_LOCK_SHIFT = 5; constexpr uint32_t RC2_SIG_PARITY_KNOWN_MASK = 0x00000040; -constexpr uint32_t RC2_SIG_PARITY_KNOWN_SHIFT = 6; constexpr uint32_t RC2_SIG_CODE_LOCK_MASK = 0x00000080; -constexpr uint32_t RC2_SIG_CODE_LOCK_SHIFT = 7; constexpr uint32_t RC2_SIG_LOCKTIME_MASK = 0x01FFFF00; -constexpr uint32_t RC2_SIG_LOCKTIME_SHIFT = 8; constexpr uint32_t RC2_SIG_CORRELATOR_TYPE_MASK = 0x1E000000; -constexpr uint32_t RC2_SIG_CORRELATOR_TYPE_SHIFT = 25; constexpr uint32_t RC2_SIG_PRIMARY_SIGNAL_MASK = 0x20000000; -constexpr uint32_t RC2_SIG_PRIMARY_SIGNAL_SHIFT = 29; constexpr uint32_t RC2_SIG_CARRIER_PHASE_MEAS_MASK = 0x40000000; -constexpr uint32_t RC2_SIG_CARRIER_PHASE_MEAS_SHIFT = 30; // For combined field ulCombinedField2 constexpr uint64_t RC2_SIG_CNO_MASK = 0x000000000000001F; constexpr uint32_t RC2_SIG_CNO_SCALE_OFFSET = 20; constexpr uint64_t RC2_SIG_PSR_STDDEV_MASK = 0x00000000000001E0; -constexpr uint32_t RC2_SIG_PSR_STDDEV_SHIFT = 5; constexpr uint64_t RC2_SIG_ADR_STDDEV_MASK = 0x0000000000001E00; -constexpr uint32_t RC2_SIG_ADR_STDDEV_SHIFT = 9; constexpr uint64_t RC2_SIG_PSR_DIFF_MASK = 0x0000000007FFE000; -constexpr uint32_t RC2_SIG_PSR_DIFF_SHIFT = 13; constexpr double RC2_SIG_PSR_DIFF_SCALE_FACTOR = 128.0; constexpr uint64_t RC2_SIG_PHASERANGE_DIFF_MASK = 0x00007FFFF8000000; -constexpr uint32_t RC2_SIG_PHASERANGE_DIFF_SHIFT = 27; constexpr double RC2_SIG_PHASERANGE_DIFF_SCALE_FACTOR = 2048.0; constexpr uint64_t RC2_SIG_DOPPLER_DIFF_MASK = 0xFFFF800000000000; -constexpr uint32_t RC2_SIG_DOPPLER_DIFF_SHIFT = 47; constexpr double RC2_SIG_DOPPLER_DIFF_SCALE_FACTOR = 256.0; constexpr uint32_t RC2_SIG_DOPPLER_DIFF_SIGNBIT_MASK = 0x00010000; constexpr uint32_t RC2_SIG_DOPPLER_DIFF_SIGNEXT_MASK = 0xFFFE0000; @@ -321,6 +287,26 @@ constexpr std::array RC4_RBLK_DOPPLER_SIGNBIT_MASK = {0x02000000, 0 constexpr std::array RC4_RBLK_DOPPLER_SIGNEXT_MASK = {0xFC000000, 0xFFFFC000}; constexpr std::array RC4_RBLK_INVALID_PSR = {137438953471, -524288}; +// replace this with std::countr_zero when C++20 is available +constexpr uint32_t CountTrailingZeros(uint64_t value) +{ + uint32_t count = 0; + while ((value & 1) == 0) + { + value >>= 1; + ++count; + } + return count; +} + +template constexpr T GetBitfield(uint64_t value, uint64_t mask) +{ + static_assert(std::is_integral::value || std::is_floating_point::value || std::is_enum_v, + "GetBitfield only returns integral, floating point, or enum types."); + // TODO: need to do some checking to ensure that the mask is valid (not too large) for the type of T + return static_cast((value & mask) >> CountTrailingZeros(mask)); +} + //----------------------------------------------------------------------- //! \enum SYSTEM //! \brief Satellite Constellation System enumerations. These can also @@ -757,46 +743,41 @@ struct ChannelTrackingStatus //! Constructor from a channel tracking status word. ChannelTrackingStatus(uint32_t uiChannelTrackingStatus_) { - eTrackingState = static_cast((uiChannelTrackingStatus_ & CTS_TRACKING_STATE_MASK) >> CTS_TRACKING_STATE_SHIFT); - uiSVChannelNumber = static_cast((uiChannelTrackingStatus_ & CTS_SV_CHANNEL_NUMBER_MASK) >> CTS_SV_CHANNEL_NUMBER_SHIFT); - eCorrelatorType = static_cast((uiChannelTrackingStatus_ & CTS_CORRELATOR_MASK) >> CTS_CORRELATOR_SHIFT); - eSatelliteSystem = static_cast((uiChannelTrackingStatus_ & CTS_SATELLITE_SYSTEM_MASK) >> CTS_SATELLITE_SYSTEM_SHIFT); - eSignalType = static_cast((uiChannelTrackingStatus_ & CTS_SIGNAL_TYPE_MASK) >> CTS_SIGNAL_TYPE_SHIFT); - - bPhaseLocked = static_cast((uiChannelTrackingStatus_ & CTS_PHASE_LOCK_MASK) >> CTS_PHASE_LOCK_SHIFT); - bParityKnown = static_cast((uiChannelTrackingStatus_ & CTS_PARITY_KNOWN_MASK) >> CTS_PARITY_KNOWN_SHIFT); - bCodeLocked = static_cast((uiChannelTrackingStatus_ & CTS_CODE_LOCKED_MASK) >> CTS_CODE_LOCKED_SHIFT); - bGrouped = static_cast((uiChannelTrackingStatus_ & CTS_GROUPING_MASK) >> CTS_GROUPING_SHIFT); - bPrimaryL1Channel = static_cast((uiChannelTrackingStatus_ & CTS_PRIMARY_L1_CHANNEL_MASK) >> CTS_PRIMARY_L1_CHANNEL_SHIFT); - bHalfCycleAdded = static_cast((uiChannelTrackingStatus_ & CTS_CARRIER_PHASE_MASK) >> CTS_CARRIER_PHASE_SHIFT); - bDigitalFilteringOnSignal = static_cast((uiChannelTrackingStatus_ & CTS_DIGITAL_FILTERING_MASK) >> CTS_DIGITAL_FILTERING_SHIFT); - bPRNLocked = static_cast((uiChannelTrackingStatus_ & CTS_PRN_LOCK_MASK) >> CTS_PRN_LOCK_SHIFT); - bChannelAssignmentForced = static_cast((uiChannelTrackingStatus_ & CTS_CHANNEL_ASSIGNMENT_MASK) >> CTS_CHANNEL_ASSIGNMENT_SHIFT); + eTrackingState = GetBitfield(uiChannelTrackingStatus_, CTS_TRACKING_STATE_MASK); + uiSVChannelNumber = GetBitfield(uiChannelTrackingStatus_, CTS_SV_CHANNEL_NUMBER_MASK); + eCorrelatorType = GetBitfield(uiChannelTrackingStatus_, CTS_CORRELATOR_MASK); + eSatelliteSystem = GetBitfield(uiChannelTrackingStatus_, CTS_SATELLITE_SYSTEM_MASK); + eSignalType = GetBitfield(uiChannelTrackingStatus_, CTS_SIGNAL_TYPE_MASK); + bPhaseLocked = GetBitfield(uiChannelTrackingStatus_, CTS_PHASE_LOCK_MASK); + bParityKnown = GetBitfield(uiChannelTrackingStatus_, CTS_PARITY_KNOWN_MASK); + bCodeLocked = GetBitfield(uiChannelTrackingStatus_, CTS_CODE_LOCKED_MASK); + bGrouped = GetBitfield(uiChannelTrackingStatus_, CTS_GROUPING_MASK); + bPrimaryL1Channel = GetBitfield(uiChannelTrackingStatus_, CTS_PRIMARY_L1_CHANNEL_MASK); + bHalfCycleAdded = GetBitfield(uiChannelTrackingStatus_, CTS_CARRIER_PHASE_MASK); + bDigitalFilteringOnSignal = GetBitfield(uiChannelTrackingStatus_, CTS_DIGITAL_FILTERING_MASK); + bPRNLocked = GetBitfield(uiChannelTrackingStatus_, CTS_PRN_LOCK_MASK); + bChannelAssignmentForced = GetBitfield(uiChannelTrackingStatus_, CTS_CHANNEL_ASSIGNMENT_MASK); } //! Constructor from the available data from a RANGECMP2 SAT/SIG block pair. //! NOTE: Some defaults exist here. ChannelTrackingStatus(const RangeCmp2SatelliteBlock& stRangeCmp2SatBlock_, const RangeCmp2SignalBlock& stRangeCmp2SigBlock_) { - bGrouped = (static_cast(stRangeCmp2SatBlock_.ulCombinedField & RC2_SAT_NUM_SIGNAL_BLOCKS_BASE_MASK) >> - RC2_SAT_NUM_SIGNAL_BLOCKS_BASE_SHIFT) > 1; - bPhaseLocked = static_cast((stRangeCmp2SigBlock_.uiCombinedField1 & RC2_SIG_PHASE_LOCK_MASK) >> RC2_SIG_PHASE_LOCK_SHIFT); - bParityKnown = static_cast((stRangeCmp2SigBlock_.uiCombinedField1 & RC2_SIG_PARITY_KNOWN_MASK) >> RC2_SIG_PARITY_KNOWN_SHIFT); - bCodeLocked = static_cast((stRangeCmp2SigBlock_.uiCombinedField1 & RC2_SIG_CODE_LOCK_MASK) >> RC2_SIG_CODE_LOCK_SHIFT); - bPrimaryL1Channel = static_cast((stRangeCmp2SigBlock_.uiCombinedField1 & RC2_SIG_PRIMARY_SIGNAL_MASK) >> RC2_SIG_PRIMARY_SIGNAL_SHIFT); - bHalfCycleAdded = - static_cast((stRangeCmp2SigBlock_.uiCombinedField1 & RC2_SIG_CARRIER_PHASE_MEAS_MASK) >> RC2_SIG_CARRIER_PHASE_MEAS_SHIFT); + bGrouped = GetBitfield(stRangeCmp2SatBlock_.ulCombinedField, RC2_SAT_NUM_SIGNAL_BLOCKS_BASE_MASK) > 1; + bPhaseLocked = GetBitfield(stRangeCmp2SigBlock_.uiCombinedField1, RC2_SIG_PHASE_LOCK_MASK); + bParityKnown = GetBitfield(stRangeCmp2SigBlock_.uiCombinedField1, RC2_SIG_PARITY_KNOWN_MASK); + bCodeLocked = GetBitfield(stRangeCmp2SigBlock_.uiCombinedField1, RC2_SIG_CODE_LOCK_MASK); + bPrimaryL1Channel = GetBitfield(stRangeCmp2SigBlock_.uiCombinedField1, RC2_SIG_PRIMARY_SIGNAL_MASK); + bHalfCycleAdded = GetBitfield(stRangeCmp2SigBlock_.uiCombinedField1, RC2_SIG_CARRIER_PHASE_MEAS_MASK); bDigitalFilteringOnSignal = false; bChannelAssignmentForced = false; bPRNLocked = false; uiSVChannelNumber = static_cast(stRangeCmp2SatBlock_.ucSVChanNumber); eTrackingState = bPrimaryL1Channel ? TRACKING_STATE::PHASE_LOCK_LOOP : TRACKING_STATE::AIDED_PHASE_LOCK_LOOP; - eCorrelatorType = - static_cast((stRangeCmp2SigBlock_.uiCombinedField1 & RC2_SIG_CORRELATOR_TYPE_MASK) >> RC2_SIG_CORRELATOR_TYPE_SHIFT); - eSatelliteSystem = SystemToSatelliteSystem( - static_cast((stRangeCmp2SatBlock_.ulCombinedField & RC2_SAT_SATELLITE_SYSTEM_ID_MASK) >> RC2_SAT_SATELLITE_SYSTEM_ID_SHIFT)); + eCorrelatorType = GetBitfield(stRangeCmp2SigBlock_.uiCombinedField1, RC2_SIG_CORRELATOR_TYPE_MASK); + eSatelliteSystem = SystemToSatelliteSystem(GetBitfield(stRangeCmp2SatBlock_.ulCombinedField, RC2_SAT_SATELLITE_SYSTEM_ID_MASK)); eSignalType = RangeCmp2SignalTypeToSignalType( - eSatelliteSystem, static_cast(stRangeCmp2SigBlock_.uiCombinedField1 & RC2_SIG_SIGNAL_TYPE_MASK)); + eSatelliteSystem, GetBitfield(stRangeCmp2SigBlock_.uiCombinedField1, RC2_SIG_SIGNAL_TYPE_MASK)); } //! Constructor from the available data from a RANGECMP4 Primary Block and Measurement Block @@ -962,11 +943,12 @@ struct ChannelTrackingStatus //! https://docs.novatel.com/OEM7/Content/Logs/RANGE.htm?Highlight=RANGE#Table_ChannelTrackingStatus [[nodiscard]] uint32_t GetAsWord() const { + // TODO: make EncodeBitfield function uint32_t uiWord = (static_cast(eTrackingState) & CTS_TRACKING_STATE_MASK) | - ((static_cast(eCorrelatorType) << CTS_CORRELATOR_SHIFT) & CTS_CORRELATOR_MASK) | - ((static_cast(eSatelliteSystem) << CTS_SATELLITE_SYSTEM_SHIFT) & CTS_SATELLITE_SYSTEM_MASK) | - ((static_cast(eSignalType) << CTS_SIGNAL_TYPE_SHIFT) & CTS_SIGNAL_TYPE_MASK) | - ((static_cast(uiSVChannelNumber) << CTS_SV_CHANNEL_NUMBER_SHIFT) & CTS_SV_CHANNEL_NUMBER_MASK); + ((static_cast(eCorrelatorType) << CountTrailingZeros(CTS_CORRELATOR_MASK)) & CTS_CORRELATOR_MASK) | + ((static_cast(eSatelliteSystem) << CountTrailingZeros(CTS_SATELLITE_SYSTEM_MASK)) & CTS_SATELLITE_SYSTEM_MASK) | + ((static_cast(eSignalType) << CountTrailingZeros(CTS_SIGNAL_TYPE_MASK)) & CTS_SIGNAL_TYPE_MASK) | + ((static_cast(uiSVChannelNumber) << CountTrailingZeros(CTS_SV_CHANNEL_NUMBER_MASK)) & CTS_SV_CHANNEL_NUMBER_MASK); if (bPhaseLocked) { uiWord |= CTS_PHASE_LOCK_MASK; } if (bParityKnown) { uiWord |= CTS_PARITY_KNOWN_MASK; } diff --git a/src/decoders/oem/src/rangecmp/range_decompressor.cpp b/src/decoders/oem/src/rangecmp/range_decompressor.cpp index 5e1683f67..266eee1f8 100644 --- a/src/decoders/oem/src/rangecmp/range_decompressor.cpp +++ b/src/decoders/oem/src/rangecmp/range_decompressor.cpp @@ -271,7 +271,7 @@ float RangeDecompressor::GetRangeCmp2LockTime(const MetaDataStruct& stMetaData_, RangeCmp2LockTimeInfo& stLocktimeInfo = ammmMyRangeCmp2LockTimes[static_cast(stMetaData_.eMeasurementSource)][eSystem_][eSignal_][static_cast(usPRN_)]; - if (uiLockTimeBits_ == (RC2_SIG_LOCKTIME_MASK >> RC2_SIG_LOCKTIME_SHIFT)) + if (uiLockTimeBits_ == (RC2_SIG_LOCKTIME_MASK >> CountTrailingZeros(RC2_SIG_LOCKTIME_MASK))) { // If the locktime was already saturated, use the stored time to add the missing offset. if (stLocktimeInfo.bLockTimeSaturated) @@ -591,20 +591,17 @@ void RangeDecompressor::RangeCmpToRange(const RangeCmp& stRangeCmpMessage_, Rang stRangeData.usPRN = static_cast(stRangeCmpData.ucPRN); // Extend the sign - auto iDoppler = static_cast(stRangeCmpData.ulDopplerFrequencyPSRField & RC_DOPPLER_FREQUENCY_MASK); + auto iDoppler = GetBitfield(stRangeCmpData.ulDopplerFrequencyPSRField, RC_DOPPLER_FREQUENCY_MASK); if ((iDoppler & RC_DOPPLER_FREQUENCY_SIGNBIT_MASK) != 0) { iDoppler |= RC_DOPPLER_FREQUENCY_SIGNEXT_MASK; } stRangeData.fDopplerFrequency = static_cast(iDoppler / RC_DOPPLER_FREQUENCY_SCALE_FACTOR); - stRangeData.dPSR = static_cast(((stRangeCmpData.ulDopplerFrequencyPSRField & RC_PSR_MEASUREMENT_MASK) >> RC_PSR_MEASUREMENT_SHIFT) / - RC_PSR_MEASUREMENT_SCALE_FACTOR); + stRangeData.dPSR = GetBitfield(stRangeCmpData.ulDopplerFrequencyPSRField, RC_PSR_MEASUREMENT_MASK) / RC_PSR_MEASUREMENT_SCALE_FACTOR; stRangeData.fPSRStdDev = stdDevPsrScaling[stRangeCmpData.ucStdDevPSRStdDevADR & RC_PSR_STDDEV_MASK]; stRangeData.fADRStdDev = - static_cast((RC_ADR_STDDEV_SCALE_OFFSET + ((stRangeCmpData.ucStdDevPSRStdDevADR & RC_ADR_STDDEV_MASK) >> RC_ADR_STDDEV_SHIFT)) / - RC_ADR_STDDEV_SCALE_FACTOR); - stRangeData.fLockTime = static_cast((stRangeCmpData.uiLockTimeCNoGLOFreq & RC_LOCK_TIME_MASK) / RC_LOCK_TIME_SCALE_FACTOR); - stRangeData.fCNo = static_cast(RC_CNO_SCALE_OFFSET + ((stRangeCmpData.uiLockTimeCNoGLOFreq & RC_CNO_MASK) >> RC_CNO_SHIFT)); - stRangeData.sGLONASSFrequency = - static_cast((stRangeCmpData.uiLockTimeCNoGLOFreq & RC_GLONASS_FREQUENCY_MASK) >> RC_GLONASS_FREQUENCY_SHIFT); + (GetBitfield(stRangeCmpData.ucStdDevPSRStdDevADR, RC_ADR_STDDEV_MASK) + RC_ADR_STDDEV_SCALE_OFFSET) / RC_ADR_STDDEV_SCALE_FACTOR; + stRangeData.fLockTime = GetBitfield(stRangeCmpData.uiLockTimeCNoGLOFreq, RC_LOCK_TIME_MASK) / RC_LOCK_TIME_SCALE_FACTOR; + stRangeData.fCNo = GetBitfield(stRangeCmpData.uiLockTimeCNoGLOFreq, RC_CNO_MASK) + RC_CNO_SCALE_OFFSET; + stRangeData.sGLONASSFrequency = GetBitfield(stRangeCmpData.uiLockTimeCNoGLOFreq, RC_GLONASS_FREQUENCY_MASK); double dWavelength = GetSignalWavelength(stChannelTrackingStatus, stRangeData.sGLONASSFrequency); @@ -642,18 +639,15 @@ void RangeDecompressor::RangeCmp2ToRange(const RangeCmp2& stRangeCmp2Message_, R const auto& stSatBlock = reinterpret_cast(stRangeCmp2Message_.aucRangeData[uiRangeDataBytesDecompressed]); // Decompress the Satellite Block - const auto eSystem = - static_cast((stSatBlock.ulCombinedField & RC2_SAT_SATELLITE_SYSTEM_ID_MASK) >> RC2_SAT_SATELLITE_SYSTEM_ID_SHIFT); - const auto ucSignalBlockCount = - static_cast((stSatBlock.ulCombinedField & RC2_SAT_NUM_SIGNAL_BLOCKS_BASE_MASK) >> RC2_SAT_NUM_SIGNAL_BLOCKS_BASE_SHIFT); + const auto eSystem = GetBitfield(stSatBlock.ulCombinedField, RC2_SAT_SATELLITE_SYSTEM_ID_MASK); + const auto ucSignalBlockCount = GetBitfield(stSatBlock.ulCombinedField, RC2_SAT_NUM_SIGNAL_BLOCKS_BASE_MASK); // Extend the sign - auto iPSRBase = static_cast((stSatBlock.ulCombinedField & RC2_SAT_SATELLITE_PSR_BASE_MASK) >> RC2_SAT_SATELLITE_PSR_BASE_SHIFT); + auto iPSRBase = GetBitfield(stSatBlock.ulCombinedField, RC2_SAT_SATELLITE_PSR_BASE_MASK); if ((iPSRBase & RC2_SAT_SATELLITE_PSR_BASE_SIGNBIT_MASK) != 0) { iPSRBase |= RC2_SAT_SATELLITE_PSR_BASE_SIGNEXT_MASK; } // Extend the sign - auto iDopplerBase = - static_cast((stSatBlock.ulCombinedField & RC2_SAT_SATELLITE_DOPPLER_BASE_MASK) >> RC2_SAT_SATELLITE_DOPPLER_BASE_SHIFT); + auto iDopplerBase = GetBitfield(stSatBlock.ulCombinedField, RC2_SAT_SATELLITE_DOPPLER_BASE_MASK); if ((iDopplerBase & RC2_SAT_SATELLITE_DOPPLER_BASE_SIGNBIT_MASK) != 0) { iDopplerBase |= RC2_SAT_SATELLITE_DOPPLER_BASE_SIGNEXT_MASK; } uiRangeDataBytesDecompressed += sizeof(RangeCmp2SatelliteBlock); @@ -663,19 +657,18 @@ void RangeDecompressor::RangeCmp2ToRange(const RangeCmp2& stRangeCmp2Message_, R { const auto& stSigBlock = reinterpret_cast(stRangeCmp2Message_.aucRangeData[uiRangeDataBytesDecompressed]); - const auto eSignalType = static_cast(stSigBlock.uiCombinedField1 & RC2_SIG_SIGNAL_TYPE_MASK); - const auto ucPSRBitfield = static_cast((stSigBlock.ullCombinedField2 & RC2_SIG_PSR_STDDEV_MASK) >> RC2_SIG_PSR_STDDEV_SHIFT); - const auto ucADRBitfield = static_cast((stSigBlock.ullCombinedField2 & RC2_SIG_ADR_STDDEV_MASK) >> RC2_SIG_ADR_STDDEV_SHIFT); - const auto uiLocktimeBits = static_cast((stSigBlock.uiCombinedField1 & RC2_SIG_LOCKTIME_MASK) >> RC2_SIG_LOCKTIME_SHIFT); + const auto eSignalType = GetBitfield(stSigBlock.uiCombinedField1, RC2_SIG_SIGNAL_TYPE_MASK); + const auto ucPSRBitfield = GetBitfield(stSigBlock.ullCombinedField2, RC2_SIG_PSR_STDDEV_MASK); + const auto ucADRBitfield = GetBitfield(stSigBlock.ullCombinedField2, RC2_SIG_ADR_STDDEV_MASK); + const auto uiLocktimeBits = GetBitfield(stSigBlock.uiCombinedField1, RC2_SIG_LOCKTIME_MASK); const auto usPRN = static_cast(stSatBlock.ucSatelliteIdentifier + (eSystem == SYSTEM::GLONASS ? GLONASS_SLOT_OFFSET - 1 : 0)); // Extend the sign - auto iDopplerBitfield = static_cast((stSigBlock.ullCombinedField2 & RC2_SIG_DOPPLER_DIFF_MASK) >> RC2_SIG_DOPPLER_DIFF_SHIFT); + auto iDopplerBitfield = GetBitfield(stSigBlock.ullCombinedField2, RC2_SIG_DOPPLER_DIFF_MASK); if ((iDopplerBitfield & RC2_SIG_DOPPLER_DIFF_SIGNBIT_MASK) != 0) { iDopplerBitfield |= RC2_SIG_DOPPLER_DIFF_SIGNEXT_MASK; } - const auto fPSRDiff = static_cast((stSigBlock.ullCombinedField2 & RC2_SIG_PSR_DIFF_MASK) >> RC2_SIG_PSR_DIFF_SHIFT); - const auto fPhaseRangeDiff = - static_cast((stSigBlock.ullCombinedField2 & RC2_SIG_PHASERANGE_DIFF_MASK) >> RC2_SIG_PHASERANGE_DIFF_SHIFT); + const auto fPSRDiff = GetBitfield(stSigBlock.ullCombinedField2, RC2_SIG_PSR_DIFF_MASK); + const auto fPhaseRangeDiff = GetBitfield(stSigBlock.ullCombinedField2, RC2_SIG_PHASERANGE_DIFF_MASK); const auto fScaledDopplerDiff = static_cast(static_cast(iDopplerBitfield) / RC2_SIG_DOPPLER_DIFF_SCALE_FACTOR); ChannelTrackingStatus stChannelTrackingStatus(stSatBlock, stSigBlock); @@ -683,7 +676,7 @@ void RangeDecompressor::RangeCmp2ToRange(const RangeCmp2& stRangeCmp2Message_, R // Construct the decompressed range data RangeData& stRangeData = stRangeMessage_.astRangeData[stRangeMessage_.uiNumberOfObservations++]; stRangeData.usPRN = usPRN; - stRangeData.sGLONASSFrequency = static_cast(stSatBlock.ulCombinedField & RC2_SAT_GLONASS_FREQUENCY_ID_MASK); + stRangeData.sGLONASSFrequency = GetBitfield(stSatBlock.ulCombinedField, RC2_SAT_GLONASS_FREQUENCY_ID_MASK); stRangeData.dPSR = static_cast(iPSRBase + static_cast(fPSRDiff / RC2_SIG_PSR_DIFF_SCALE_FACTOR)); stRangeData.fPSRStdDev = stdDevPsrScaling[ucPSRBitfield]; stRangeData.dADR = -static_cast(iPSRBase) + @@ -692,7 +685,7 @@ void RangeDecompressor::RangeCmp2ToRange(const RangeCmp2& stRangeCmp2Message_, R stRangeData.fADRStdDev = stdDevAdrScaling[ucADRBitfield]; stRangeData.fDopplerFrequency = static_cast(iDopplerBase + fScaledDopplerDiff) / static_cast(mmTheRangeCmp2SignalScalingMapping[eSystem][eSignalType]); - stRangeData.fCNo = RC2_SIG_CNO_SCALE_OFFSET + static_cast(stSigBlock.ullCombinedField2 & RC2_SIG_CNO_MASK); + stRangeData.fCNo = RC2_SIG_CNO_SCALE_OFFSET + GetBitfield(stSigBlock.ullCombinedField2, RC2_SIG_CNO_MASK); stRangeData.fLockTime = GetRangeCmp2LockTime(stMetaData_, uiLocktimeBits, stChannelTrackingStatus.eSatelliteSystem, stChannelTrackingStatus.eSignalType, usPRN); stRangeData.uiChannelTrackingStatus = stChannelTrackingStatus.GetAsWord(); From f0f1d61016aeb378feb3288cdacbf2444670900a Mon Sep 17 00:00:00 2001 From: Jonathan McDermid Date: Fri, 25 Oct 2024 22:19:19 -0600 Subject: [PATCH 3/6] more rangecmp refactoring --- .../decoders/oem/rangecmp/common.hpp | 72 +++++------ .../oem/src/rangecmp/range_decompressor.cpp | 118 ++++++++---------- src/decoders/oem/test/range_cmp_test.cpp | 1 - 3 files changed, 86 insertions(+), 105 deletions(-) diff --git a/include/novatel_edie/decoders/oem/rangecmp/common.hpp b/include/novatel_edie/decoders/oem/rangecmp/common.hpp index d20274883..f027b088a 100644 --- a/include/novatel_edie/decoders/oem/rangecmp/common.hpp +++ b/include/novatel_edie/decoders/oem/rangecmp/common.hpp @@ -159,16 +159,16 @@ constexpr uint32_t CTS_CHANNEL_ASSIGNMENT_MASK = 0x80000000; constexpr uint64_t RC_DOPPLER_FREQUENCY_MASK = 0x000000000FFFFFFF; constexpr uint32_t RC_DOPPLER_FREQUENCY_SIGNBIT_MASK = 0x08000000; constexpr uint32_t RC_DOPPLER_FREQUENCY_SIGNEXT_MASK = 0xF0000000; -constexpr float RC_DOPPLER_FREQUENCY_SCALE_FACTOR = 256.0F; +constexpr uint32_t RC_DOPPLER_FREQUENCY_SHIFT = 8; constexpr uint64_t RC_PSR_MEASUREMENT_MASK = 0xFFFFFFFFF0000000; -constexpr double RC_PSR_MEASUREMENT_SCALE_FACTOR = 128.0; -constexpr double RC_ADR_SCALE_FACTOR = 256.0; +constexpr uint32_t RC_PSR_MEASUREMENT_SHIFT = 7; +constexpr uint32_t RC_ADR_SHIFT = 8; constexpr uint32_t RC_PSR_STDDEV_MASK = 0x0F; constexpr uint32_t RC_ADR_STDDEV_MASK = 0xF0; -constexpr double RC_ADR_STDDEV_SCALE_FACTOR = 512.0; +constexpr uint32_t RC_ADR_STDDEV_SHIFT = 9; constexpr uint32_t RC_ADR_STDDEV_SCALE_OFFSET = 1; constexpr uint32_t RC_LOCK_TIME_MASK = 0x001FFFFF; -constexpr double RC_LOCK_TIME_SCALE_FACTOR = 32.0; +constexpr uint32_t RC_LOCK_TIME_SHIFT = 5; constexpr uint32_t RC_CNO_MASK = 0x03E00000; constexpr uint32_t RC_CNO_SCALE_OFFSET = 20; constexpr uint32_t RC_GLONASS_FREQUENCY_MASK = 0xFC000000; @@ -209,11 +209,11 @@ constexpr uint32_t RC2_SIG_CNO_SCALE_OFFSET = 20; constexpr uint64_t RC2_SIG_PSR_STDDEV_MASK = 0x00000000000001E0; constexpr uint64_t RC2_SIG_ADR_STDDEV_MASK = 0x0000000000001E00; constexpr uint64_t RC2_SIG_PSR_DIFF_MASK = 0x0000000007FFE000; -constexpr double RC2_SIG_PSR_DIFF_SCALE_FACTOR = 128.0; +constexpr uint32_t RC2_SIG_PSR_DIFF_SHIFT = 7; constexpr uint64_t RC2_SIG_PHASERANGE_DIFF_MASK = 0x00007FFFF8000000; -constexpr double RC2_SIG_PHASERANGE_DIFF_SCALE_FACTOR = 2048.0; +constexpr uint32_t RC2_SIG_PHASERANGE_DIFF_SHIFT = 11; constexpr uint64_t RC2_SIG_DOPPLER_DIFF_MASK = 0xFFFF800000000000; -constexpr double RC2_SIG_DOPPLER_DIFF_SCALE_FACTOR = 256.0; +constexpr uint32_t RC2_SIG_DOPPLER_DIFF_SHIFT = 8; constexpr uint32_t RC2_SIG_DOPPLER_DIFF_SIGNBIT_MASK = 0x00010000; constexpr uint32_t RC2_SIG_DOPPLER_DIFF_SIGNEXT_MASK = 0xFFFE0000; @@ -288,25 +288,29 @@ constexpr std::array RC4_RBLK_DOPPLER_SIGNEXT_MASK = {0xFC000000, 0 constexpr std::array RC4_RBLK_INVALID_PSR = {137438953471, -524288}; // replace this with std::countr_zero when C++20 is available -constexpr uint32_t CountTrailingZeros(uint64_t value) +constexpr uint32_t Lsb(uint64_t value) { - uint32_t count = 0; - while ((value & 1) == 0) - { - value >>= 1; - ++count; - } - return count; + if (value == 0) { return 64; } // Indicate no bits are set + +#ifdef _MSC_VER + unsigned long index; + _BitScanForward64(&index, value); + return static_cast(index); +#else + return __builtin_ctzll(value); +#endif } template constexpr T GetBitfield(uint64_t value, uint64_t mask) { - static_assert(std::is_integral::value || std::is_floating_point::value || std::is_enum_v, - "GetBitfield only returns integral, floating point, or enum types."); + static_assert(std::is_integral::value || std::is_enum_v, "GetBitfield only returns integral or enum types."); // TODO: need to do some checking to ensure that the mask is valid (not too large) for the type of T - return static_cast((value & mask) >> CountTrailingZeros(mask)); + return static_cast((value & mask) >> Lsb(mask)); } +// TODO: need to do some checking to ensure that the result is valid +constexpr uint32_t EncodeBitfield(uint32_t value, uint32_t mask) { return value << Lsb(mask) & mask; } + //----------------------------------------------------------------------- //! \enum SYSTEM //! \brief Satellite Constellation System enumerations. These can also @@ -490,7 +494,7 @@ enum class SIGNAL_TYPE struct RangeCmp2 { uint32_t uiNumberOfRangeDataBytes{0}; - uint8_t aucRangeData[(RANGE_RECORD_MAX * (sizeof(RangeCmp2SatelliteBlock) + sizeof(RangeCmp2SignalBlock)))]{0}; + uint8_t aucRangeData[RANGE_RECORD_MAX * (sizeof(RangeCmp2SatelliteBlock) + sizeof(RangeCmp2SignalBlock))]{}; RangeCmp2() = default; }; @@ -546,10 +550,10 @@ struct RangeCmp4MeasurementSignalBlock //----------------------------------------------------------------------- struct RangeCmp4LocktimeInfo { - double dLocktimeMilliseconds{0.0}; // The current running locktime for this observation. - double dLastBitfieldChangeMilliseconds{0.0}; // The last time (milliseconds from OEM header) locktime was updated. - uint8_t ucLocktimeBits{UINT8_MAX}; // The last recorded bit pattern. - bool bLocktimeAbsolute{false}; // Is the lock time absolute or relative? + double dLocktimeMilliseconds{0.0}; // The current running locktime for this observation. + double dLastBitfieldChangeMilliseconds{0.0}; // The last time (milliseconds from OEM header) locktime was updated. + uint8_t ucLocktimeBits{std::numeric_limits::max()}; // The last recorded bit pattern. + bool bLocktimeAbsolute{false}; // Is the lock time absolute or relative? RangeCmp4LocktimeInfo() = default; }; @@ -780,8 +784,7 @@ struct ChannelTrackingStatus eSatelliteSystem, GetBitfield(stRangeCmp2SigBlock_.uiCombinedField1, RC2_SIG_SIGNAL_TYPE_MASK)); } - //! Constructor from the available data from a RANGECMP4 Primary Block and Measurement Block - //! pair. + //! Constructor from the available data from a RANGECMP4 Primary Block and Measurement Block pair. ChannelTrackingStatus(SYSTEM eSystem_, rangecmp4::SIGNAL_TYPE eSignalType_, const RangeCmp4MeasurementSignalBlock& stMeasurementBlock_) { // Defaults that cannot be determined: @@ -790,8 +793,7 @@ struct ChannelTrackingStatus bPRNLocked = false; bChannelAssignmentForced = false; bDigitalFilteringOnSignal = false; - bGrouped = false; // Note that bGrouped can be changed once the number of signals for this - // PRN have been determined. + bGrouped = false; // Note that bGrouped can be changed once the number of signals for this PRN have been determined. eSatelliteSystem = SystemToSatelliteSystem(eSystem_); eSignalType = RangeCmp4SignalTypeToSignalType(eSatelliteSystem, eSignalType_); @@ -938,17 +940,15 @@ struct ChannelTrackingStatus : SATELLITE_SYSTEM::OTHER; } - //! Combine the channel tracking status fields into a single 4-byte value according to - //! documentation: + //! Combine the channel tracking status fields into a single 4-byte value according to documentation: //! https://docs.novatel.com/OEM7/Content/Logs/RANGE.htm?Highlight=RANGE#Table_ChannelTrackingStatus [[nodiscard]] uint32_t GetAsWord() const { - // TODO: make EncodeBitfield function - uint32_t uiWord = (static_cast(eTrackingState) & CTS_TRACKING_STATE_MASK) | - ((static_cast(eCorrelatorType) << CountTrailingZeros(CTS_CORRELATOR_MASK)) & CTS_CORRELATOR_MASK) | - ((static_cast(eSatelliteSystem) << CountTrailingZeros(CTS_SATELLITE_SYSTEM_MASK)) & CTS_SATELLITE_SYSTEM_MASK) | - ((static_cast(eSignalType) << CountTrailingZeros(CTS_SIGNAL_TYPE_MASK)) & CTS_SIGNAL_TYPE_MASK) | - ((static_cast(uiSVChannelNumber) << CountTrailingZeros(CTS_SV_CHANNEL_NUMBER_MASK)) & CTS_SV_CHANNEL_NUMBER_MASK); + uint32_t uiWord = EncodeBitfield(static_cast(eTrackingState), CTS_TRACKING_STATE_MASK) | + EncodeBitfield(static_cast(eCorrelatorType), CTS_CORRELATOR_MASK) | + EncodeBitfield(static_cast(eSatelliteSystem), CTS_SATELLITE_SYSTEM_MASK) | + EncodeBitfield(static_cast(eSignalType), CTS_SIGNAL_TYPE_MASK) | + EncodeBitfield(uiSVChannelNumber, CTS_SV_CHANNEL_NUMBER_MASK); if (bPhaseLocked) { uiWord |= CTS_PHASE_LOCK_MASK; } if (bParityKnown) { uiWord |= CTS_PARITY_KNOWN_MASK; } diff --git a/src/decoders/oem/src/rangecmp/range_decompressor.cpp b/src/decoders/oem/src/rangecmp/range_decompressor.cpp index 266eee1f8..bf1991a25 100644 --- a/src/decoders/oem/src/rangecmp/range_decompressor.cpp +++ b/src/decoders/oem/src/rangecmp/range_decompressor.cpp @@ -151,11 +151,11 @@ double RangeDecompressor::GetSignalWavelength(const ChannelTrackingStatus& stCha : 0.0; case System::GLONASS: return stChannelStatus_.eSignalType == Signal::GLONASS_L1CA - ? SPEED_OF_LIGHT / (FREQUENCY_HZ_GLO_L1 + (sGLONASSFrequency_ * GLONASS_L1_FREQUENCY_SCALE_HZ)) + ? SPEED_OF_LIGHT / (FREQUENCY_HZ_GLO_L1 + sGLONASSFrequency_ * GLONASS_L1_FREQUENCY_SCALE_HZ) : stChannelStatus_.eSignalType == Signal::GLONASS_L2CA - ? SPEED_OF_LIGHT / (FREQUENCY_HZ_GLO_L2 + (sGLONASSFrequency_ * GLONASS_L2_FREQUENCY_SCALE_HZ)) + ? SPEED_OF_LIGHT / (FREQUENCY_HZ_GLO_L2 + sGLONASSFrequency_ * GLONASS_L2_FREQUENCY_SCALE_HZ) : stChannelStatus_.eSignalType == Signal::GLONASS_L2P - ? SPEED_OF_LIGHT / (FREQUENCY_HZ_GLO_L2 + (sGLONASSFrequency_ * GLONASS_L2_FREQUENCY_SCALE_HZ)) + ? SPEED_OF_LIGHT / (FREQUENCY_HZ_GLO_L2 + sGLONASSFrequency_ * GLONASS_L2_FREQUENCY_SCALE_HZ) : stChannelStatus_.eSignalType == Signal::GLONASS_L3Q ? WAVELENGTH_GLO_L3 : 0.0; case System::SBAS: @@ -267,17 +267,14 @@ float RangeDecompressor::GetRangeCmp2LockTime(const MetaDataStruct& stMetaData_, ChannelTrackingStatus::SATELLITE_SYSTEM eSystem_, ChannelTrackingStatus::SIGNAL_TYPE eSignal_, uint16_t usPRN_) { - auto fLocktimeMilliseconds = static_cast(uiLockTimeBits_); + float fLocktimeMilliseconds = uiLockTimeBits_; RangeCmp2LockTimeInfo& stLocktimeInfo = ammmMyRangeCmp2LockTimes[static_cast(stMetaData_.eMeasurementSource)][eSystem_][eSignal_][static_cast(usPRN_)]; - if (uiLockTimeBits_ == (RC2_SIG_LOCKTIME_MASK >> CountTrailingZeros(RC2_SIG_LOCKTIME_MASK))) + if (uiLockTimeBits_ == RC2_SIG_LOCKTIME_MASK >> Lsb(RC2_SIG_LOCKTIME_MASK)) { // If the locktime was already saturated, use the stored time to add the missing offset. - if (stLocktimeInfo.bLockTimeSaturated) - { - fLocktimeMilliseconds += static_cast(stMetaData_.dMilliseconds - stLocktimeInfo.dLockTimeSaturatedMilliseconds); - } + if (stLocktimeInfo.bLockTimeSaturated) { fLocktimeMilliseconds += stMetaData_.dMilliseconds - stLocktimeInfo.dLockTimeSaturatedMilliseconds; } // If the locktime is not already saturated, store this information if this observation is seen again. else { @@ -370,7 +367,7 @@ float RangeDecompressor::GetRangeCmp4LockTime(const MetaDataStruct& stMetaData_, ammmMyRangeCmp4LockTimes[static_cast(stMetaData_.eMeasurementSource)][eSystem_][eSignal_][uiPRN_]; // Is the locktime relative and has a ullBitfield change been found? - if ((!stLocktimeInfo.bLocktimeAbsolute) && (ucLockTimeBits_ != stLocktimeInfo.ucLocktimeBits)) + if (!stLocktimeInfo.bLocktimeAbsolute && ucLockTimeBits_ != stLocktimeInfo.ucLocktimeBits) { // Set locktime as absolute if bits are 0 or transitioning from relative to absolute. if (ucLockTimeBits_ == 0 || (stLocktimeInfo.ucLocktimeBits != 0xFF && ucLockTimeBits_ > stLocktimeInfo.ucLocktimeBits)) @@ -414,7 +411,7 @@ float RangeDecompressor::GetRangeCmp4LockTime(const MetaDataStruct& stMetaData_, } stLocktimeInfo.dLocktimeMilliseconds = stMetaData_.dMilliseconds - stLocktimeInfo.dLastBitfieldChangeMilliseconds + lockTime[stLocktimeInfo.ucLocktimeBits]; - return static_cast(stLocktimeInfo.dLocktimeMilliseconds) / SEC_TO_MILLI_SEC; + return stLocktimeInfo.dLocktimeMilliseconds / SEC_TO_MILLI_SEC; } //------------------------------------------------------------------------------ @@ -443,12 +440,12 @@ void RangeDecompressor::DecompressReferenceBlock(unsigned char** ppucData_, uint if (iDopplerBitfield & RC4_RBLK_DOPPLER_SIGNBIT_MASK[bSecondary]) { iDopplerBitfield |= RC4_RBLK_DOPPLER_SIGNEXT_MASK[bSecondary]; } stRefBlock_.bValidPSR = llPSRBitfield != RC4_RBLK_INVALID_PSR[bSecondary]; - stRefBlock_.dPSR = static_cast(llPSRBitfield) * RC4_SIG_BLK_PSR_SCALE_FACTOR + + stRefBlock_.dPSR = llPSRBitfield * RC4_SIG_BLK_PSR_SCALE_FACTOR + (bSecondary ? astMyLastPrimaryReferenceBlocks[static_cast(eMeasurementSource_)].dPSR : 0); stRefBlock_.bValidPhaseRange = iPhaseRangeBitfield != RC4_SIG_RBLK_INVALID_PHASERANGE; - stRefBlock_.dPhaseRange = static_cast(iPhaseRangeBitfield) * RC4_SIG_BLK_PHASERANGE_SCALE_FACTOR + stRefBlock_.dPSR; + stRefBlock_.dPhaseRange = iPhaseRangeBitfield * RC4_SIG_BLK_PHASERANGE_SCALE_FACTOR + stRefBlock_.dPSR; stRefBlock_.bValidDoppler = iDopplerBitfield != RC4_PSIG_RBLK_INVALID_DOPPLER; - stRefBlock_.dDoppler = static_cast(iDopplerBitfield) * RC4_SIG_BLK_DOPPLER_SCALE_FACTOR + + stRefBlock_.dDoppler = iDopplerBitfield * RC4_SIG_BLK_DOPPLER_SCALE_FACTOR + (bSecondary ? astMyLastPrimaryReferenceBlocks[static_cast(eMeasurementSource_)].dDoppler : 0); if constexpr (!bSecondary) @@ -474,20 +471,19 @@ void RangeDecompressor::DecompressDifferentialBlock(unsigned char** ppucData_, u stDiffBlock_.ucPSRBitfield = ExtractBitfield(ppucData_, uiBytesLeft_, uiBitOffset_, RC4_SIG_BLK_PSR_STDDEV_BITS); stDiffBlock_.ucADRBitfield = ExtractBitfield(ppucData_, uiBytesLeft_, uiBitOffset_, RC4_SIG_BLK_ADR_STDDEV_BITS); - auto iPSRBitfield = ExtractBitfield(ppucData_, uiBytesLeft_, uiBitOffset_, RC4_SIG_DBLK_PSR_BITS); - if (iPSRBitfield & RC4_SIG_DBLK_PSR_SIGNBIT_MASK) { iPSRBitfield |= RC4_SIG_DBLK_PSR_SIGNEXT_MASK; } - auto iPhaseRangeBitfield = ExtractBitfield(ppucData_, uiBytesLeft_, uiBitOffset_, RC4_SIG_DBLK_PHASERANGE_BITS); - if (iPhaseRangeBitfield & RC4_SIG_DBLK_PHASERANGE_SIGNBIT_MASK) { iPhaseRangeBitfield |= RC4_SIG_DBLK_PHASERANGE_SIGNEXT_MASK; } - auto iDopplerBitfield = ExtractBitfield(ppucData_, uiBytesLeft_, uiBitOffset_, RC4_DBLK_DOPPLER_BITS[bSecondary]); - if (iDopplerBitfield & RC4_DBLK_DOPPLER_SIGNBIT_MASK[bSecondary]) { iDopplerBitfield |= RC4_DBLK_DOPPLER_SIGNEXT_MASK[bSecondary]; } - - stDiffBlock_.bValidPSR = iPSRBitfield != RC4_SIG_DBLK_INVALID_PSR; - stDiffBlock_.dPSR = static_cast(iPSRBitfield) * RC4_SIG_BLK_PSR_SCALE_FACTOR + stRefBlock_.dPSR + (stRefBlock_.dDoppler * dSecondOffset_); - stDiffBlock_.bValidPhaseRange = iPhaseRangeBitfield != RC4_SIG_DBLK_INVALID_PHASERANGE; - stDiffBlock_.dPhaseRange = static_cast(iPhaseRangeBitfield) * RC4_SIG_BLK_PHASERANGE_SCALE_FACTOR + stRefBlock_.dPhaseRange + - stRefBlock_.dDoppler * dSecondOffset_; - stDiffBlock_.bValidDoppler = iDopplerBitfield != RC4_DBLK_INVALID_DOPPLER[bSecondary]; - stDiffBlock_.dDoppler = static_cast(iDopplerBitfield) * RC4_SIG_BLK_DOPPLER_SCALE_FACTOR + stRefBlock_.dDoppler; + auto iPSR = ExtractBitfield(ppucData_, uiBytesLeft_, uiBitOffset_, RC4_SIG_DBLK_PSR_BITS); + if (iPSR & RC4_SIG_DBLK_PSR_SIGNBIT_MASK) { iPSR |= RC4_SIG_DBLK_PSR_SIGNEXT_MASK; } + auto iPhaseRange = ExtractBitfield(ppucData_, uiBytesLeft_, uiBitOffset_, RC4_SIG_DBLK_PHASERANGE_BITS); + if (iPhaseRange & RC4_SIG_DBLK_PHASERANGE_SIGNBIT_MASK) { iPhaseRange |= RC4_SIG_DBLK_PHASERANGE_SIGNEXT_MASK; } + auto iDoppler = ExtractBitfield(ppucData_, uiBytesLeft_, uiBitOffset_, RC4_DBLK_DOPPLER_BITS[bSecondary]); + if (iDoppler & RC4_DBLK_DOPPLER_SIGNBIT_MASK[bSecondary]) { iDoppler |= RC4_DBLK_DOPPLER_SIGNEXT_MASK[bSecondary]; } + + stDiffBlock_.bValidPSR = iPSR != RC4_SIG_DBLK_INVALID_PSR; + stDiffBlock_.dPSR = iPSR * RC4_SIG_BLK_PSR_SCALE_FACTOR + stRefBlock_.dPSR + (stRefBlock_.dDoppler * dSecondOffset_); + stDiffBlock_.bValidPhaseRange = iPhaseRange != RC4_SIG_DBLK_INVALID_PHASERANGE; + stDiffBlock_.dPhaseRange = iPhaseRange * RC4_SIG_BLK_PHASERANGE_SCALE_FACTOR + stRefBlock_.dPhaseRange + stRefBlock_.dDoppler * dSecondOffset_; + stDiffBlock_.bValidDoppler = iDoppler != RC4_DBLK_INVALID_DOPPLER[bSecondary]; + stDiffBlock_.dDoppler = iDoppler * RC4_SIG_BLK_DOPPLER_SCALE_FACTOR + stRefBlock_.dDoppler; } //------------------------------------------------------------------------------ @@ -517,8 +513,7 @@ void RangeDecompressor::PopulateNextRangeData(RangeData& stRangeData_, const Ran constexpr std::array stdDevAdrScaling = {0.003F, 0.005F, 0.007F, 0.009F, 0.012F, 0.016F, 0.022F, 0.029F, 0.039F, 0.052F, 0.070F, 0.093F, 0.124F, 0.166F, 0.222F, 0.222F}; - double dSignalWavelength = - GetSignalWavelength(stChannelStatus_, static_cast(cGLONASSFrequencyNumber_ - GLONASS_FREQUENCY_NUMBER_OFFSET)); + double dSignalWavelength = GetSignalWavelength(stChannelStatus_, cGLONASSFrequencyNumber_ - GLONASS_FREQUENCY_NUMBER_OFFSET); //! Some logic for PRN offsets based on the constellation. See documentation: //! https://docs.novatel.com/OEM7/Content/Logs/RANGECMP4.htm#Measurem @@ -529,19 +524,17 @@ void RangeDecompressor::PopulateNextRangeData(RangeData& stRangeData_, const Ran // GLONASS_SLOT_UNKNOWN_UPPER_LIMIT - cGLONASSFrequencyNumber_. However, this // would output the PRN as an actual valid Slot ID, which is not true. We will // set this to 0 here because 0 is considered an unknown/invalid GLONASS Slot ID. - stRangeData_.usPRN = (GLONASS_SLOT_UNKNOWN_LOWER_LIMIT <= uiPRN_ && uiPRN_ <= GLONASS_SLOT_UNKNOWN_UPPER_LIMIT) - ? 0 - : static_cast(uiPRN_) + GLONASS_SLOT_OFFSET - 1; + stRangeData_.usPRN = + (GLONASS_SLOT_UNKNOWN_LOWER_LIMIT <= uiPRN_ && uiPRN_ <= GLONASS_SLOT_UNKNOWN_UPPER_LIMIT) ? 0 : uiPRN_ + GLONASS_SLOT_OFFSET - 1; break; case ChannelTrackingStatus::SATELLITE_SYSTEM::SBAS: - stRangeData_.usPRN = (SBAS_PRN_OFFSET_120_LOWER_LIMIT <= uiPRN_ && uiPRN_ <= SBAS_PRN_OFFSET_120_UPPER_LIMIT) - ? static_cast(uiPRN_ + SBAS_PRN_OFFSET_120 - 1) - : (SBAS_PRN_OFFSET_130_LOWER_LIMIT <= uiPRN_ && uiPRN_ <= SBAS_PRN_OFFSET_130_UPPER_LIMIT) - ? static_cast(uiPRN_ + SBAS_PRN_OFFSET_130 - 1) - : 0; + stRangeData_.usPRN = + (SBAS_PRN_OFFSET_120_LOWER_LIMIT <= uiPRN_ && uiPRN_ <= SBAS_PRN_OFFSET_120_UPPER_LIMIT) ? uiPRN_ + SBAS_PRN_OFFSET_120 - 1 + : (SBAS_PRN_OFFSET_130_LOWER_LIMIT <= uiPRN_ && uiPRN_ <= SBAS_PRN_OFFSET_130_UPPER_LIMIT) ? uiPRN_ + SBAS_PRN_OFFSET_130 - 1 + : 0; break; - case ChannelTrackingStatus::SATELLITE_SYSTEM::QZSS: stRangeData_.usPRN = static_cast(uiPRN_ + QZSS_PRN_OFFSET - 1); break; - default: stRangeData_.usPRN = static_cast(uiPRN_); break; + case ChannelTrackingStatus::SATELLITE_SYSTEM::QZSS: stRangeData_.usPRN = uiPRN_ + QZSS_PRN_OFFSET - 1; break; + default: stRangeData_.usPRN = uiPRN_; break; } if (stChannelStatus_.eSatelliteSystem != ChannelTrackingStatus::SATELLITE_SYSTEM::GLONASS && stRangeData_.usPRN == 0) @@ -550,13 +543,12 @@ void RangeDecompressor::PopulateNextRangeData(RangeData& stRangeData_, const Ran } // any fields flagged as invalid are set to NaN and appear in the log as such - stRangeData_.sGLONASSFrequency = static_cast(static_cast(cGLONASSFrequencyNumber_)); + stRangeData_.sGLONASSFrequency = static_cast(cGLONASSFrequencyNumber_); stRangeData_.dPSR = stBlock_.bValidPSR ? stBlock_.dPSR : std::numeric_limits::quiet_NaN(); stRangeData_.fPSRStdDev = stdDevPsrScaling[stBlock_.ucPSRBitfield]; stRangeData_.dADR = stBlock_.bValidPhaseRange ? -stBlock_.dPhaseRange / dSignalWavelength : std::numeric_limits::quiet_NaN(); stRangeData_.fADRStdDev = stdDevAdrScaling[stBlock_.ucADRBitfield]; - stRangeData_.fDopplerFrequency = - stBlock_.bValidDoppler ? static_cast(-stBlock_.dDoppler / dSignalWavelength) : std::numeric_limits::quiet_NaN(); + stRangeData_.fDopplerFrequency = stBlock_.bValidDoppler ? -stBlock_.dDoppler / dSignalWavelength : std::numeric_limits::quiet_NaN(); stRangeData_.fCNo = stBlock_.fCNo; stRangeData_.fLockTime = GetRangeCmp4LockTime(stMetaData_, stBlock_.ucLockTimeBitfield, stChannelStatus_.eSatelliteSystem, stChannelStatus_.eSignalType, uiPRN_); @@ -588,24 +580,22 @@ void RangeDecompressor::RangeCmpToRange(const RangeCmp& stRangeCmpMessage_, Rang ChannelTrackingStatus stChannelTrackingStatus(stRangeCmpData.uiChannelTrackingStatus); stRangeData.uiChannelTrackingStatus = stRangeCmpData.uiChannelTrackingStatus; - stRangeData.usPRN = static_cast(stRangeCmpData.ucPRN); + stRangeData.usPRN = stRangeCmpData.ucPRN; - // Extend the sign auto iDoppler = GetBitfield(stRangeCmpData.ulDopplerFrequencyPSRField, RC_DOPPLER_FREQUENCY_MASK); if ((iDoppler & RC_DOPPLER_FREQUENCY_SIGNBIT_MASK) != 0) { iDoppler |= RC_DOPPLER_FREQUENCY_SIGNEXT_MASK; } - stRangeData.fDopplerFrequency = static_cast(iDoppler / RC_DOPPLER_FREQUENCY_SCALE_FACTOR); + stRangeData.fDopplerFrequency = iDoppler >> RC_DOPPLER_FREQUENCY_SHIFT; - stRangeData.dPSR = GetBitfield(stRangeCmpData.ulDopplerFrequencyPSRField, RC_PSR_MEASUREMENT_MASK) / RC_PSR_MEASUREMENT_SCALE_FACTOR; + stRangeData.dPSR = GetBitfield(stRangeCmpData.ulDopplerFrequencyPSRField, RC_PSR_MEASUREMENT_MASK) >> RC_PSR_MEASUREMENT_SHIFT; stRangeData.fPSRStdDev = stdDevPsrScaling[stRangeCmpData.ucStdDevPSRStdDevADR & RC_PSR_STDDEV_MASK]; stRangeData.fADRStdDev = - (GetBitfield(stRangeCmpData.ucStdDevPSRStdDevADR, RC_ADR_STDDEV_MASK) + RC_ADR_STDDEV_SCALE_OFFSET) / RC_ADR_STDDEV_SCALE_FACTOR; - stRangeData.fLockTime = GetBitfield(stRangeCmpData.uiLockTimeCNoGLOFreq, RC_LOCK_TIME_MASK) / RC_LOCK_TIME_SCALE_FACTOR; - stRangeData.fCNo = GetBitfield(stRangeCmpData.uiLockTimeCNoGLOFreq, RC_CNO_MASK) + RC_CNO_SCALE_OFFSET; + (GetBitfield(stRangeCmpData.ucStdDevPSRStdDevADR, RC_ADR_STDDEV_MASK) + RC_ADR_STDDEV_SCALE_OFFSET) >> RC_ADR_STDDEV_SHIFT; + stRangeData.fLockTime = GetBitfield(stRangeCmpData.uiLockTimeCNoGLOFreq, RC_LOCK_TIME_MASK) >> RC_LOCK_TIME_SHIFT; + stRangeData.fCNo = GetBitfield(stRangeCmpData.uiLockTimeCNoGLOFreq, RC_CNO_MASK) + RC_CNO_SCALE_OFFSET; stRangeData.sGLONASSFrequency = GetBitfield(stRangeCmpData.uiLockTimeCNoGLOFreq, RC_GLONASS_FREQUENCY_MASK); double dWavelength = GetSignalWavelength(stChannelTrackingStatus, stRangeData.sGLONASSFrequency); - - stRangeData.dADR = static_cast(stRangeCmpData.uiADR) / RC_ADR_SCALE_FACTOR; + stRangeData.dADR = stRangeCmpData.uiADR >> RC_ADR_SHIFT; double dADRRolls = ((stRangeData.dPSR / dWavelength) + stRangeData.dADR) / MAX_VALUE; stRangeData.dADR -= MAX_VALUE * static_cast(std::round(dADRRolls)); } @@ -638,15 +628,12 @@ void RangeDecompressor::RangeCmp2ToRange(const RangeCmp2& stRangeCmp2Message_, R { const auto& stSatBlock = reinterpret_cast(stRangeCmp2Message_.aucRangeData[uiRangeDataBytesDecompressed]); - // Decompress the Satellite Block const auto eSystem = GetBitfield(stSatBlock.ulCombinedField, RC2_SAT_SATELLITE_SYSTEM_ID_MASK); const auto ucSignalBlockCount = GetBitfield(stSatBlock.ulCombinedField, RC2_SAT_NUM_SIGNAL_BLOCKS_BASE_MASK); - // Extend the sign auto iPSRBase = GetBitfield(stSatBlock.ulCombinedField, RC2_SAT_SATELLITE_PSR_BASE_MASK); if ((iPSRBase & RC2_SAT_SATELLITE_PSR_BASE_SIGNBIT_MASK) != 0) { iPSRBase |= RC2_SAT_SATELLITE_PSR_BASE_SIGNEXT_MASK; } - // Extend the sign auto iDopplerBase = GetBitfield(stSatBlock.ulCombinedField, RC2_SAT_SATELLITE_DOPPLER_BASE_MASK); if ((iDopplerBase & RC2_SAT_SATELLITE_DOPPLER_BASE_SIGNBIT_MASK) != 0) { iDopplerBase |= RC2_SAT_SATELLITE_DOPPLER_BASE_SIGNEXT_MASK; } @@ -655,37 +642,33 @@ void RangeDecompressor::RangeCmp2ToRange(const RangeCmp2& stRangeCmp2Message_, R // Decompress the Signal Blocks associated with the Satellite Block for (uint8_t ucSignalBlockIndex = 0; ucSignalBlockIndex < ucSignalBlockCount; ucSignalBlockIndex++) { + // Decompress the signal block const auto& stSigBlock = reinterpret_cast(stRangeCmp2Message_.aucRangeData[uiRangeDataBytesDecompressed]); const auto eSignalType = GetBitfield(stSigBlock.uiCombinedField1, RC2_SIG_SIGNAL_TYPE_MASK); const auto ucPSRBitfield = GetBitfield(stSigBlock.ullCombinedField2, RC2_SIG_PSR_STDDEV_MASK); const auto ucADRBitfield = GetBitfield(stSigBlock.ullCombinedField2, RC2_SIG_ADR_STDDEV_MASK); const auto uiLocktimeBits = GetBitfield(stSigBlock.uiCombinedField1, RC2_SIG_LOCKTIME_MASK); - const auto usPRN = static_cast(stSatBlock.ucSatelliteIdentifier + (eSystem == SYSTEM::GLONASS ? GLONASS_SLOT_OFFSET - 1 : 0)); + const auto usPRN = stSatBlock.ucSatelliteIdentifier + (eSystem == SYSTEM::GLONASS ? GLONASS_SLOT_OFFSET - 1 : 0); - // Extend the sign auto iDopplerBitfield = GetBitfield(stSigBlock.ullCombinedField2, RC2_SIG_DOPPLER_DIFF_MASK); if ((iDopplerBitfield & RC2_SIG_DOPPLER_DIFF_SIGNBIT_MASK) != 0) { iDopplerBitfield |= RC2_SIG_DOPPLER_DIFF_SIGNEXT_MASK; } - const auto fPSRDiff = GetBitfield(stSigBlock.ullCombinedField2, RC2_SIG_PSR_DIFF_MASK); - const auto fPhaseRangeDiff = GetBitfield(stSigBlock.ullCombinedField2, RC2_SIG_PHASERANGE_DIFF_MASK); - const auto fScaledDopplerDiff = static_cast(static_cast(iDopplerBitfield) / RC2_SIG_DOPPLER_DIFF_SCALE_FACTOR); - ChannelTrackingStatus stChannelTrackingStatus(stSatBlock, stSigBlock); // Construct the decompressed range data RangeData& stRangeData = stRangeMessage_.astRangeData[stRangeMessage_.uiNumberOfObservations++]; stRangeData.usPRN = usPRN; stRangeData.sGLONASSFrequency = GetBitfield(stSatBlock.ulCombinedField, RC2_SAT_GLONASS_FREQUENCY_ID_MASK); - stRangeData.dPSR = static_cast(iPSRBase + static_cast(fPSRDiff / RC2_SIG_PSR_DIFF_SCALE_FACTOR)); + stRangeData.dPSR = iPSRBase + (GetBitfield(stSigBlock.ullCombinedField2, RC2_SIG_PSR_DIFF_MASK) >> RC2_SIG_PSR_DIFF_SHIFT); stRangeData.fPSRStdDev = stdDevPsrScaling[ucPSRBitfield]; - stRangeData.dADR = -static_cast(iPSRBase) + - static_cast(fPhaseRangeDiff / RC2_SIG_PHASERANGE_DIFF_SCALE_FACTOR) / - (GetSignalWavelength(stChannelTrackingStatus, (stRangeData.sGLONASSFrequency - GLONASS_FREQUENCY_NUMBER_OFFSET))); + stRangeData.dADR = + -((iPSRBase + (GetBitfield(stSigBlock.ullCombinedField2, RC2_SIG_PHASERANGE_DIFF_MASK) >> RC2_SIG_PHASERANGE_DIFF_SHIFT)) / + GetSignalWavelength(stChannelTrackingStatus, stRangeData.sGLONASSFrequency - GLONASS_FREQUENCY_NUMBER_OFFSET)); stRangeData.fADRStdDev = stdDevAdrScaling[ucADRBitfield]; stRangeData.fDopplerFrequency = - static_cast(iDopplerBase + fScaledDopplerDiff) / static_cast(mmTheRangeCmp2SignalScalingMapping[eSystem][eSignalType]); - stRangeData.fCNo = RC2_SIG_CNO_SCALE_OFFSET + GetBitfield(stSigBlock.ullCombinedField2, RC2_SIG_CNO_MASK); + (iDopplerBase + (iDopplerBitfield >> RC2_SIG_DOPPLER_DIFF_SHIFT)) / mmTheRangeCmp2SignalScalingMapping[eSystem][eSignalType]; + stRangeData.fCNo = RC2_SIG_CNO_SCALE_OFFSET + GetBitfield(stSigBlock.ullCombinedField2, RC2_SIG_CNO_MASK); stRangeData.fLockTime = GetRangeCmp2LockTime(stMetaData_, uiLocktimeBits, stChannelTrackingStatus.eSatelliteSystem, stChannelTrackingStatus.eSignalType, usPRN); stRangeData.uiChannelTrackingStatus = stChannelTrackingStatus.GetAsWord(); @@ -736,7 +719,6 @@ void RangeDecompressor::RangeCmp4ToRange(unsigned char* pucData_, Range& stRange // Collect the satellite PRNs tracked in this satellite system. for (uint8_t ucBitPosition = 0; ucBitPosition < RC4_SATELLITES_BITS; ucBitPosition++) { - // Note that ucBitPosition contains the PRN value at this point. if ((ullSatellites & (1ULL << ucBitPosition)) != 0) { vPRNs.push_back(ucBitPosition + 1); } // Bit position is PRN - 1, so + 1 here } diff --git a/src/decoders/oem/test/range_cmp_test.cpp b/src/decoders/oem/test/range_cmp_test.cpp index 71c7b8b4e..af22b0ac2 100644 --- a/src/decoders/oem/test/range_cmp_test.cpp +++ b/src/decoders/oem/test/range_cmp_test.cpp @@ -267,7 +267,6 @@ TEST_F(RangeCmpTest, DISABLED_DECOMPRESS_RANGECMPA_3) stMetaData.uiLength = sizeof(aucCompressedData)-1; ASSERT_EQ(STATUS::SUCCESS, pclMyRangeDecompressor->Decompress(reinterpret_cast(aucCompressionBuffer), sizeof(aucCompressionBuffer), stMetaData)); - std::cout << "DECOMPRESS_RANGECMPA4_DIFF_2" << aucCompressionBuffer << '\n'; ASSERT_EQ(sizeof(aucDecompressedData)-1, stMetaData.uiLength); ASSERT_EQ(0, memcmp(aucCompressionBuffer, aucDecompressedData, stMetaData.uiLength)); } From f93db8f273050c6ce46154c964c5f12f15d31eb9 Mon Sep 17 00:00:00 2001 From: Jonathan McDermid Date: Wed, 6 Nov 2024 20:35:54 -0700 Subject: [PATCH 4/6] refactor rxconfig --- .../oem/rxconfig/rxconfig_handler.hpp | 21 ++-- .../oem/src/rxconfig/rxconfig_handler.cpp | 110 +++++++----------- 2 files changed, 49 insertions(+), 82 deletions(-) diff --git a/include/novatel_edie/decoders/oem/rxconfig/rxconfig_handler.hpp b/include/novatel_edie/decoders/oem/rxconfig/rxconfig_handler.hpp index 4ccb8085d..6a33bbb88 100644 --- a/include/novatel_edie/decoders/oem/rxconfig/rxconfig_handler.hpp +++ b/include/novatel_edie/decoders/oem/rxconfig/rxconfig_handler.hpp @@ -67,16 +67,6 @@ class RxConfigHandler std::unique_ptr pcMyFrameBuffer; std::unique_ptr pcMyEncodeBuffer; - template - [[nodiscard]] static bool PrintToBuffer(char** ppcBuffer_, uint32_t& uiBytesLeft_, const char* szFormat_, Args&&... args_) - { - uint32_t uiBytesBuffered = std::snprintf(*ppcBuffer_, uiBytesLeft_, szFormat_, std::forward(args_)...); - if (uiBytesLeft_ < uiBytesBuffered) { return false; } - *ppcBuffer_ += uiBytesBuffered; - uiBytesLeft_ -= uiBytesBuffered; - return true; - } - static bool IsRxConfigTypeMsg(uint16_t usMessageId_); public: @@ -105,14 +95,14 @@ class RxConfigHandler // //! \return A shared_ptr to the spdlog::logger. //---------------------------------------------------------------------------- - [[nodiscard]] std::shared_ptr GetLogger() const; + [[nodiscard]] std::shared_ptr GetLogger() const { return pclMyLogger; } //---------------------------------------------------------------------------- //! \brief Set the level of detail produced by the internal logger. // //! \param[in] eLevel_ The logging level to enable. //---------------------------------------------------------------------------- - void SetLoggerLevel(spdlog::level::level_enum eLevel_) const; + void SetLoggerLevel(spdlog::level::level_enum eLevel_) const { pclMyLogger->set_level(eLevel_); } //---------------------------------------------------------------------------- //! \brief Write bytes to the RxConfigHandler to be converted. @@ -122,7 +112,7 @@ class RxConfigHandler // //! \return The number of bytes successfully written to the RxConfigHandler. //---------------------------------------------------------------------------- - uint32_t Write(unsigned char* pucData_, uint32_t uiDataSize_); + uint32_t Write(unsigned char* pucData_, uint32_t uiDataSize_) { return clMyFramer.Write(pucData_, uiDataSize_); } //---------------------------------------------------------------------------- //! \brief Read and convert an RXCONFIG message from the handler. @@ -153,7 +143,10 @@ class RxConfigHandler // //! \return The number of bytes flushed from the internal Framer. //---------------------------------------------------------------------------- - uint32_t Flush(unsigned char* pucBuffer_ = nullptr, uint32_t uiBufferSize_ = uiInternalBufferSize); + uint32_t Flush(unsigned char* pucBuffer_ = nullptr, uint32_t uiBufferSize_ = uiInternalBufferSize) + { + return clMyFramer.Flush(pucBuffer_, uiBufferSize_); + } }; } // namespace novatel::edie::oem diff --git a/src/decoders/oem/src/rxconfig/rxconfig_handler.cpp b/src/decoders/oem/src/rxconfig/rxconfig_handler.cpp index 0079f7f87..cd3e65530 100644 --- a/src/decoders/oem/src/rxconfig/rxconfig_handler.cpp +++ b/src/decoders/oem/src/rxconfig/rxconfig_handler.cpp @@ -58,24 +58,15 @@ void RxConfigHandler::LoadJsonDb(JsonReader* pclJsonDB_) } // ------------------------------------------------------------------------------------------------------- -void RxConfigHandler::SetLoggerLevel(spdlog::level::level_enum eLevel_) const { pclMyLogger->set_level(eLevel_); } - -// ------------------------------------------------------------------------------------------------------- -std::shared_ptr RxConfigHandler::GetLogger() const { return pclMyLogger; } - -//----------------------------------------------------------------------- bool RxConfigHandler::IsRxConfigTypeMsg(uint16_t usMessageId_) { return (usMessageId_ == US_RX_CONFIG_MSG_ID || usMessageId_ == US_RX_CONFIG_USER_MSG_ID); } -// ------------------------------------------------------------------------------------------------------- -uint32_t RxConfigHandler::Write(unsigned char* pucData_, uint32_t uiDataSize_) { return clMyFramer.Write(pucData_, uiDataSize_); } - // ------------------------------------------------------------------------------------------------------- STATUS RxConfigHandler::Convert(MessageDataStruct& stRxConfigMessageData_, MetaDataStruct& stRxConfigMetaData_, MessageDataStruct& stEmbeddedMessageData_, - MetaDataStruct& stEmbeddedMetaData_, ENCODE_FORMAT eEncodeFormat_) + MetaDataStruct& stEmbeddedMetaData_, ENCODE_FORMAT eFormat_) { IntermediateHeader stRxConfigHeader; IntermediateHeader stEmbeddedHeader; @@ -117,45 +108,39 @@ RxConfigHandler::Convert(MessageDataStruct& stRxConfigMessageData_, MetaDataStru if (eStatus != STATUS::SUCCESS) { return eStatus; } // Encode the RXCONFIG log. - unsigned char* pucTempEncodeBuffer = pcMyEncodeBuffer.get(); - uint32_t uiMyBufferBytesRemaining = uiInternalBufferSize; + unsigned char* pucEncodeBuffer = pcMyEncodeBuffer.get(); + uint32_t uiBytesLeft = uiInternalBufferSize; - if (eEncodeFormat_ == ENCODE_FORMAT::JSON && - !PrintToBuffer(reinterpret_cast(&pucTempEncodeBuffer), uiMyBufferBytesRemaining, R"({"header":)")) + if (eFormat_ == ENCODE_FORMAT::JSON && !PrintToBuffer(reinterpret_cast(&pucEncodeBuffer), uiBytesLeft, R"({"header":)")) { return STATUS::BUFFER_FULL; } - eStatus = clMyEncoder.EncodeHeader(&pucTempEncodeBuffer, uiMyBufferBytesRemaining, stRxConfigHeader, stRxConfigMessageData_, stRxConfigMetaData_, - eEncodeFormat_); + eStatus = clMyEncoder.EncodeHeader(&pucEncodeBuffer, uiBytesLeft, stRxConfigHeader, stRxConfigMessageData_, stRxConfigMetaData_, eFormat_); if (eStatus == STATUS::NO_DEFINITION) { return STATUS::NO_DEFINITION_EMBEDDED; } if (eStatus != STATUS::SUCCESS) { return eStatus; } - pucTempEncodeBuffer += stRxConfigMessageData_.uiMessageHeaderLength; + pucEncodeBuffer += stRxConfigMessageData_.uiMessageHeaderLength; - if (eEncodeFormat_ == ENCODE_FORMAT::JSON && - !PrintToBuffer(reinterpret_cast(&pucTempEncodeBuffer), uiMyBufferBytesRemaining, R"(,"body":)")) + if (eFormat_ == ENCODE_FORMAT::JSON && !PrintToBuffer(reinterpret_cast(&pucEncodeBuffer), uiBytesLeft, R"(,"body":)")) { return STATUS::BUFFER_FULL; } - stEmbeddedMessageData_.pucMessage = pucTempEncodeBuffer; - stRxConfigMessageData_.pucMessageBody = pucTempEncodeBuffer; + stEmbeddedMessageData_.pucMessage = pucEncodeBuffer; + stRxConfigMessageData_.pucMessageBody = pucEncodeBuffer; // This is just dummy args that we must pass to the encoder. They will not be used. uint32_t uiCRC = 0; - switch (eEncodeFormat_) + switch (eFormat_) { case ENCODE_FORMAT::JSON: - if (!PrintToBuffer(reinterpret_cast(&pucTempEncodeBuffer), uiMyBufferBytesRemaining, R"({"embedded_header":)")) - { - return STATUS::BUFFER_FULL; - } + if (!PrintToBuffer(reinterpret_cast(&pucEncodeBuffer), uiBytesLeft, R"({"embedded_header":)")) { return STATUS::BUFFER_FULL; } break; case ENCODE_FORMAT::ABBREV_ASCII: - if (!PrintToBuffer(reinterpret_cast(&pucTempEncodeBuffer), uiMyBufferBytesRemaining, szAbbrevAsciiEmbeddedHeaderPrefix)) + if (!PrintToBuffer(reinterpret_cast(&pucEncodeBuffer), uiBytesLeft, szAbbrevAsciiEmbeddedHeaderPrefix)) { return STATUS::BUFFER_FULL; } @@ -164,20 +149,16 @@ RxConfigHandler::Convert(MessageDataStruct& stRxConfigMessageData_, MetaDataStru default: break; } - eStatus = clMyEncoder.EncodeHeader(&pucTempEncodeBuffer, uiMyBufferBytesRemaining, stEmbeddedHeader, stEmbeddedMessageData_, stEmbeddedMetaData_, - eEncodeFormat_, true); + eStatus = clMyEncoder.EncodeHeader(&pucEncodeBuffer, uiBytesLeft, stEmbeddedHeader, stEmbeddedMessageData_, stEmbeddedMetaData_, eFormat_, true); if (eStatus != STATUS::SUCCESS) { return eStatus; } - pucTempEncodeBuffer += stEmbeddedMessageData_.uiMessageHeaderLength; - uiMyBufferBytesRemaining -= stEmbeddedMessageData_.uiMessageHeaderLength; + pucEncodeBuffer += stEmbeddedMessageData_.uiMessageHeaderLength; + uiBytesLeft -= stEmbeddedMessageData_.uiMessageHeaderLength; - switch (eEncodeFormat_) + switch (eFormat_) { case ENCODE_FORMAT::JSON: - if (!PrintToBuffer(reinterpret_cast(&pucTempEncodeBuffer), uiMyBufferBytesRemaining, R"(,"embedded_body":)")) - { - return STATUS::BUFFER_FULL; - } + if (!PrintToBuffer(reinterpret_cast(&pucEncodeBuffer), uiBytesLeft, R"(,"embedded_body":)")) { return STATUS::BUFFER_FULL; } break; case ENCODE_FORMAT::ABBREV_ASCII: @@ -191,7 +172,7 @@ RxConfigHandler::Convert(MessageDataStruct& stRxConfigMessageData_, MetaDataStru // it back in the message MessageHeaderLength count. stEmbeddedMessageData_.uiMessageHeaderLength++; - if (!PrintToBuffer(reinterpret_cast(&pucTempEncodeBuffer), uiMyBufferBytesRemaining, "\r\n")) { return STATUS::BUFFER_FULL; } + if (!PrintToBuffer(reinterpret_cast(&pucEncodeBuffer), uiBytesLeft, "\r\n")) { return STATUS::BUFFER_FULL; } stEmbeddedMessageData_.uiMessageHeaderLength++; break; @@ -199,80 +180,73 @@ RxConfigHandler::Convert(MessageDataStruct& stRxConfigMessageData_, MetaDataStru default: break; } - eStatus = clMyEncoder.EncodeBody(&pucTempEncodeBuffer, uiMyBufferBytesRemaining, stEmbeddedMessage, stEmbeddedMessageData_, stEmbeddedMetaData_, - eEncodeFormat_); + eStatus = clMyEncoder.EncodeBody(&pucEncodeBuffer, uiBytesLeft, stEmbeddedMessage, stEmbeddedMessageData_, stEmbeddedMetaData_, eFormat_); if (eStatus != STATUS::SUCCESS) { return eStatus; } - uiMyBufferBytesRemaining -= stEmbeddedMessageData_.uiMessageBodyLength; - pucTempEncodeBuffer += stEmbeddedMessageData_.uiMessageBodyLength; + uiBytesLeft -= stEmbeddedMessageData_.uiMessageBodyLength; + pucEncodeBuffer += stEmbeddedMessageData_.uiMessageBodyLength; // The last CRC would have been written correctly. Pull it out, flip it, put it back in. // This will be done differently depending on how we encoded the message. - switch (eEncodeFormat_) + switch (eFormat_) { case ENCODE_FORMAT::ASCII: // Move back over CRLF. - pucTempEncodeBuffer -= 2; - uiMyBufferBytesRemaining += 2; + pucEncodeBuffer -= 2; + uiBytesLeft += 2; stEmbeddedMessageData_.uiMessageBodyLength -= 2; // Move back over the CRC. - pucTempEncodeBuffer -= OEM4_ASCII_CRC_LENGTH; - uiMyBufferBytesRemaining += OEM4_ASCII_CRC_LENGTH; + pucEncodeBuffer -= OEM4_ASCII_CRC_LENGTH; + uiBytesLeft += OEM4_ASCII_CRC_LENGTH; // Grab the CRC from the encode buffer and invert it. - uiCRC = strtoul(reinterpret_cast(pucTempEncodeBuffer), nullptr, 16) ^ 0xFFFFFFFF; - if (!PrintToBuffer(reinterpret_cast(&pucTempEncodeBuffer), uiMyBufferBytesRemaining, "%08x", uiCRC)) { return STATUS::BUFFER_FULL; } + uiCRC = strtoul(reinterpret_cast(pucEncodeBuffer), nullptr, 16) ^ 0xFFFFFFFF; + if (!PrintToBuffer(reinterpret_cast(&pucEncodeBuffer), uiBytesLeft, "%08x", uiCRC)) { return STATUS::BUFFER_FULL; } break; case ENCODE_FORMAT::BINARY: // Move back over the CRC. - pucTempEncodeBuffer -= OEM4_BINARY_CRC_LENGTH; - uiMyBufferBytesRemaining += OEM4_BINARY_CRC_LENGTH; + pucEncodeBuffer -= OEM4_BINARY_CRC_LENGTH; + uiBytesLeft += OEM4_BINARY_CRC_LENGTH; // Grab the CRC from the encode buffer and invert it. - uiCRC = *(reinterpret_cast(pucTempEncodeBuffer)) ^ 0xFFFFFFFF; - if (!CopyToBuffer(&pucTempEncodeBuffer, uiMyBufferBytesRemaining, &uiCRC)) { return STATUS::BUFFER_FULL; } + uiCRC = *(reinterpret_cast(pucEncodeBuffer)) ^ 0xFFFFFFFF; + if (!CopyToBuffer(&pucEncodeBuffer, uiBytesLeft, &uiCRC)) { return STATUS::BUFFER_FULL; } break; case ENCODE_FORMAT::JSON: - if (!PrintToBuffer(reinterpret_cast(&pucTempEncodeBuffer), uiMyBufferBytesRemaining, R"(})")) { return STATUS::BUFFER_FULL; } + if (!PrintToBuffer(reinterpret_cast(&pucEncodeBuffer), uiBytesLeft, R"(})")) { return STATUS::BUFFER_FULL; } break; default: break; } - stEmbeddedMessageData_.uiMessageLength = static_cast(pucTempEncodeBuffer - stRxConfigMessageData_.pucMessageBody); + stEmbeddedMessageData_.uiMessageLength = static_cast(pucEncodeBuffer - stRxConfigMessageData_.pucMessageBody); // Put the final CRC at the end. - switch (eEncodeFormat_) + switch (eFormat_) { case ENCODE_FORMAT::ASCII: - uiCRC = CalculateBlockCrc32(static_cast(pucTempEncodeBuffer - (pcMyEncodeBuffer.get() + 1)), 0, pcMyEncodeBuffer.get() + 1); - if (!PrintToBuffer(reinterpret_cast(&pucTempEncodeBuffer), uiMyBufferBytesRemaining, "*%08x\r\n", uiCRC)) - { - return STATUS::BUFFER_FULL; - } + uiCRC = CalculateBlockCrc32(static_cast(pucEncodeBuffer - (pcMyEncodeBuffer.get() + 1)), 0, pcMyEncodeBuffer.get() + 1); + if (!PrintToBuffer(reinterpret_cast(&pucEncodeBuffer), uiBytesLeft, "*%08x\r\n", uiCRC)) { return STATUS::BUFFER_FULL; } break; case ENCODE_FORMAT::BINARY: - uiCRC = CalculateBlockCrc32(static_cast(pucTempEncodeBuffer - pcMyEncodeBuffer.get()), 0, pcMyEncodeBuffer.get()); - if (!CopyToBuffer(&pucTempEncodeBuffer, uiMyBufferBytesRemaining, &uiCRC)) { return STATUS::BUFFER_FULL; } + uiCRC = CalculateBlockCrc32(static_cast(pucEncodeBuffer - pcMyEncodeBuffer.get()), 0, pcMyEncodeBuffer.get()); + if (!CopyToBuffer(&pucEncodeBuffer, uiBytesLeft, &uiCRC)) { return STATUS::BUFFER_FULL; } break; default: break; } - stRxConfigMessageData_.uiMessageBodyLength = static_cast(pucTempEncodeBuffer - stRxConfigMessageData_.pucMessageBody); + stRxConfigMessageData_.uiMessageBodyLength = static_cast(pucEncodeBuffer - stRxConfigMessageData_.pucMessageBody); // Add the closing '}' character, but don't count it as part of the message body length. - if (eEncodeFormat_ == ENCODE_FORMAT::JSON && !PrintToBuffer(reinterpret_cast(&pucTempEncodeBuffer), uiMyBufferBytesRemaining, R"(})")) + if (eFormat_ == ENCODE_FORMAT::JSON && !PrintToBuffer(reinterpret_cast(&pucEncodeBuffer), uiBytesLeft, R"(})")) { return STATUS::BUFFER_FULL; } stRxConfigMessageData_.pucMessage = pcMyEncodeBuffer.get(); - stRxConfigMessageData_.uiMessageLength = static_cast(pucTempEncodeBuffer - pcMyEncodeBuffer.get()); + stRxConfigMessageData_.uiMessageLength = static_cast(pucEncodeBuffer - pcMyEncodeBuffer.get()); return STATUS::SUCCESS; } - -// ------------------------------------------------------------------------------------------------------- -uint32_t RxConfigHandler::Flush(unsigned char* pucBuffer_, uint32_t uiBufferSize_) { return clMyFramer.Flush(pucBuffer_, uiBufferSize_); } From 60ecc6a397348210fae11496a6cf8afec2811aa0 Mon Sep 17 00:00:00 2001 From: Jonathan McDermid Date: Tue, 12 Nov 2024 14:31:00 -0700 Subject: [PATCH 5/6] use double in rangecmp --- .../oem/rangecmp/range_decompressor.hpp | 8 ++-- .../oem/src/rangecmp/range_decompressor.cpp | 44 +++++++++---------- 2 files changed, 26 insertions(+), 26 deletions(-) diff --git a/include/novatel_edie/decoders/oem/rangecmp/range_decompressor.hpp b/include/novatel_edie/decoders/oem/rangecmp/range_decompressor.hpp index 0ed4350fb..459b607b4 100644 --- a/include/novatel_edie/decoders/oem/rangecmp/range_decompressor.hpp +++ b/include/novatel_edie/decoders/oem/rangecmp/range_decompressor.hpp @@ -104,10 +104,10 @@ class RangeDecompressor ammmMyReferenceBlocks[static_cast(MEASUREMENT_SOURCE::MAX)]; static double GetSignalWavelength(const ChannelTrackingStatus& stChannelStatus_, int16_t sGLONASSFrequency_); - float GetRangeCmp2LockTime(const MetaDataStruct& stMetaData_, uint32_t uiLockTimeBits_, ChannelTrackingStatus::SATELLITE_SYSTEM eSystem_, - ChannelTrackingStatus::SIGNAL_TYPE eSignal_, uint16_t usPRN_); - float GetRangeCmp4LockTime(const MetaDataStruct& stMetaData_, uint8_t ucLockTimeBits_, ChannelTrackingStatus::SATELLITE_SYSTEM eSystem_, - ChannelTrackingStatus::SIGNAL_TYPE eSignal_, uint32_t uiPRN_); + double GetRangeCmp2LockTime(const MetaDataStruct& stMetaData_, uint32_t uiLockTimeBits_, ChannelTrackingStatus::SATELLITE_SYSTEM eSystem_, + ChannelTrackingStatus::SIGNAL_TYPE eSignal_, uint16_t usPRN_); + double GetRangeCmp4LockTime(const MetaDataStruct& stMetaData_, uint8_t ucLockTimeBits_, ChannelTrackingStatus::SATELLITE_SYSTEM eSystem_, + ChannelTrackingStatus::SIGNAL_TYPE eSignal_, uint32_t uiPRN_); template void DecompressReferenceBlock(unsigned char** ppucData_, uint32_t& uiBytesLeft_, uint32_t& uiBitOffset_, RangeCmp4MeasurementSignalBlock& stRefBlock_, MEASUREMENT_SOURCE eMeasurementSource_); diff --git a/src/decoders/oem/src/rangecmp/range_decompressor.cpp b/src/decoders/oem/src/rangecmp/range_decompressor.cpp index bf1991a25..29ad1fcd4 100644 --- a/src/decoders/oem/src/rangecmp/range_decompressor.cpp +++ b/src/decoders/oem/src/rangecmp/range_decompressor.cpp @@ -263,11 +263,11 @@ template uint64_t RangeDecompressor::ExtractBitfield(unsigned char**, //! observation, and may not be a true representation of the time the //! observation has actually been locked. //------------------------------------------------------------------------------ -float RangeDecompressor::GetRangeCmp2LockTime(const MetaDataStruct& stMetaData_, uint32_t uiLockTimeBits_, - ChannelTrackingStatus::SATELLITE_SYSTEM eSystem_, ChannelTrackingStatus::SIGNAL_TYPE eSignal_, - uint16_t usPRN_) +double RangeDecompressor::GetRangeCmp2LockTime(const MetaDataStruct& stMetaData_, uint32_t uiLockTimeBits_, + ChannelTrackingStatus::SATELLITE_SYSTEM eSystem_, ChannelTrackingStatus::SIGNAL_TYPE eSignal_, + uint16_t usPRN_) { - float fLocktimeMilliseconds = uiLockTimeBits_; + double fLocktimeMilliseconds = uiLockTimeBits_; RangeCmp2LockTimeInfo& stLocktimeInfo = ammmMyRangeCmp2LockTimes[static_cast(stMetaData_.eMeasurementSource)][eSystem_][eSignal_][static_cast(usPRN_)]; @@ -347,20 +347,20 @@ float RangeDecompressor::GetRangeCmp2LockTime(const MetaDataStruct& stMetaData_, //! +--------------------------------------------------------------------> //! Header time (t) //------------------------------------------------------------------------------ -float RangeDecompressor::GetRangeCmp4LockTime(const MetaDataStruct& stMetaData_, uint8_t ucLockTimeBits_, - ChannelTrackingStatus::SATELLITE_SYSTEM eSystem_, ChannelTrackingStatus::SIGNAL_TYPE eSignal_, - uint32_t uiPRN_) +double RangeDecompressor::GetRangeCmp4LockTime(const MetaDataStruct& stMetaData_, uint8_t ucLockTimeBits_, + ChannelTrackingStatus::SATELLITE_SYSTEM eSystem_, ChannelTrackingStatus::SIGNAL_TYPE eSignal_, + uint32_t uiPRN_) { //----------------------------------------------------------------------- - //! List of pre-defined floats used as translations for RANGECMP4 lock + //! List of pre-defined doubles used as translations for RANGECMP4 lock //! time values defined in the RANGECMP4 documentation: //! https://docs.novatel.com/OEM7/Content/Logs/RANGECMP4.htm?Highlight=Range#Lock //! NOTE: These values are the lower bound of the range representations. //! For more information on decompressing locktime bitfields, see the //! comment block above RangeDecompressor::DetermineRangeCmp4ObservationLocktime(). //----------------------------------------------------------------------- - constexpr std::array lockTime = {0.0F, 16.0F, 32.0F, 64.0F, 128.0F, 256.0F, 512.0F, 1024.0F, - 2048.0F, 4096.0F, 8192.0F, 16384.0F, 32768.0F, 65536.0F, 131072.0F, 262144.0F}; + constexpr std::array lockTime = {0.0, 16.0, 32.0, 64.0, 128.0, 256.0, 512.0, 1024.0, + 2048.0, 4096.0, 8192.0, 16384.0, 32768.0, 65536.0, 131072.0, 262144.0}; // Store the locktime if it is different than the once we have currently. RangeCmp4LocktimeInfo& stLocktimeInfo = @@ -495,23 +495,23 @@ void RangeDecompressor::PopulateNextRangeData(RangeData& stRangeData_, const Ran char cGLONASSFrequencyNumber_) { //----------------------------------------------------------------------- - //! List of pre-defined floats used as translations for RANGECMP4 PSR + //! List of pre-defined doubles used as translations for RANGECMP4 PSR //! standard deviation values defined in the RANGECMP4 documentation: //! https://docs.novatel.com/OEM7/Content/Logs/RANGECMP4.htm?Highlight=Range#Pseudora //----------------------------------------------------------------------- - constexpr std::array stdDevPsrScaling = {0.020F, 0.030F, 0.045F, 0.066F, 0.099F, 0.148F, 0.220F, 0.329F, - 0.491F, 0.732F, 1.092F, 1.629F, 2.430F, 3.625F, 5.409F, 5.409F}; + constexpr std::array stdDevPsrScaling = {0.020, 0.030, 0.045, 0.066, 0.099, 0.148, 0.220, 0.329, + 0.491, 0.732, 1.092, 1.629, 2.430, 3.625, 5.409, 5.409}; //----------------------------------------------------------------------- - //! List of pre-defined floats used as translations for RANGECMP4 ADR + //! List of pre-defined doubles used as translations for RANGECMP4 ADR //! standard deviation values defined in the RANGECMP4 documentation: //! https://docs.novatel.com/OEM7/Content/Logs/RANGECMP4.htm?Highlight=Range#ADR //! Note: LSD have been removed to reduce rouning errors in the range log. //! ADR STD is only 3 decimal places and could round up causing values //! to be greater than values in the table. //----------------------------------------------------------------------- - constexpr std::array stdDevAdrScaling = {0.003F, 0.005F, 0.007F, 0.009F, 0.012F, 0.016F, 0.022F, 0.029F, - 0.039F, 0.052F, 0.070F, 0.093F, 0.124F, 0.166F, 0.222F, 0.222F}; + constexpr std::array stdDevAdrScaling = {0.003, 0.005, 0.007, 0.009, 0.012, 0.016, 0.022, 0.029, + 0.039, 0.052, 0.070, 0.093, 0.124, 0.166, 0.222, 0.222}; double dSignalWavelength = GetSignalWavelength(stChannelStatus_, cGLONASSFrequencyNumber_ - GLONASS_FREQUENCY_NUMBER_OFFSET); @@ -565,8 +565,8 @@ void RangeDecompressor::RangeCmpToRange(const RangeCmp& stRangeCmpMessage_, Rang //! in the RANGECMP documentation: //! https://docs.novatel.com/OEM7/Content/Logs/RANGECMP.htm?Highlight=rangecmp#StdDevPSRValues //----------------------------------------------------------------------- - constexpr std::array stdDevPsrScaling = {0.050F, 0.075F, 0.113F, 0.169F, 0.253F, 0.380F, 0.570F, 0.854F, - 1.281F, 2.375F, 4.750F, 9.500F, 19.000F, 38.000F, 76.000F, 152.000F}; + constexpr std::array stdDevPsrScaling = {0.050, 0.075, 0.113, 0.169, 0.253, 0.380, 0.570, 0.854, + 1.281, 2.375, 4.750, 9.500, 19.000, 38.000, 76.000, 152.000}; stRangeMessage_.uiNumberOfObservations = stRangeCmpMessage_.uiNumberOfObservations; @@ -611,16 +611,16 @@ void RangeDecompressor::RangeCmp2ToRange(const RangeCmp2& stRangeCmp2Message_, R //! in the RANGECMP2 documentation: //! https://docs.novatel.com/OEM7/Content/Logs/RANGECMP2.htm?Highlight=RANGECMP2#StdDevPSRScaling //----------------------------------------------------------------------- - constexpr std::array stdDevPsrScaling = {0.020F, 0.030F, 0.045F, 0.066F, 0.099F, 0.148F, 0.220F, 0.329F, - 0.491F, 0.732F, 1.092F, 1.629F, 2.430F, 3.625F, 5.409F, 5.409F}; + constexpr std::array stdDevPsrScaling = {0.020, 0.030, 0.045, 0.066, 0.099, 0.148, 0.220, 0.329, + 0.491, 0.732, 1.092, 1.629, 2.430, 3.625, 5.409, 5.409}; //----------------------------------------------------------------------- //! Table used to expand the scaled Accumulated Doppler Range STDs. This //! is defined in the RANGECMP2 documentation: //! https://docs.novatel.com/OEM7/Content/Logs/RANGECMP2.htm?Highlight=RANGECMP2#StdDevADRScaling //----------------------------------------------------------------------- - constexpr std::array stdDevAdrScaling = {0.00391F, 0.00521F, 0.00696F, 0.00929F, 0.01239F, 0.01654F, 0.02208F, 0.02947F, - 0.03933F, 0.05249F, 0.07006F, 0.09350F, 0.12480F, 0.16656F, 0.22230F, 0.22230F}; + constexpr std::array stdDevAdrScaling = {0.00391, 0.00521, 0.00696, 0.00929, 0.01239, 0.01654, 0.02208, 0.02947, + 0.03933, 0.05249, 0.07006, 0.09350, 0.12480, 0.16656, 0.22230, 0.22230}; stRangeMessage_.uiNumberOfObservations = 0; uint32_t uiRangeDataBytesDecompressed = 0; From ec6dfd4e684f852228d5db9887c2f1c9ecd80c56 Mon Sep 17 00:00:00 2001 From: Jonathan McDermid Date: Wed, 13 Nov 2024 21:31:33 -0700 Subject: [PATCH 6/6] use switches --- .../decoders/oem/rangecmp/common.hpp | 230 +++++++++++------- .../oem/src/rangecmp/range_decompressor.cpp | 106 ++++---- 2 files changed, 205 insertions(+), 131 deletions(-) diff --git a/include/novatel_edie/decoders/oem/rangecmp/common.hpp b/include/novatel_edie/decoders/oem/rangecmp/common.hpp index f027b088a..261758b24 100644 --- a/include/novatel_edie/decoders/oem/rangecmp/common.hpp +++ b/include/novatel_edie/decoders/oem/rangecmp/common.hpp @@ -823,51 +823,79 @@ struct ChannelTrackingStatus switch (eSystem_) { case SATELLITE_SYSTEM::GPS: - return eSignalType_ == rangecmp2::SIGNAL_TYPE::GPS_L1CA ? SIGNAL_TYPE::GPS_L1CA - : eSignalType_ == rangecmp2::SIGNAL_TYPE::GPS_L2Y ? SIGNAL_TYPE::GPS_L2Y - : eSignalType_ == rangecmp2::SIGNAL_TYPE::GPS_L2CM ? SIGNAL_TYPE::GPS_L2CM - : eSignalType_ == rangecmp2::SIGNAL_TYPE::GPS_L2P ? SIGNAL_TYPE::GPS_L2P - : eSignalType_ == rangecmp2::SIGNAL_TYPE::GPS_L5Q ? SIGNAL_TYPE::GPS_L5Q - : eSignalType_ == rangecmp2::SIGNAL_TYPE::GPS_L1C ? SIGNAL_TYPE::GPS_L1CP - : SIGNAL_TYPE::UNKNOWN; + switch (eSignalType_) + { + case rangecmp2::SIGNAL_TYPE::GPS_L1CA: return SIGNAL_TYPE::GPS_L1CA; + case rangecmp2::SIGNAL_TYPE::GPS_L2Y: return SIGNAL_TYPE::GPS_L2Y; + case rangecmp2::SIGNAL_TYPE::GPS_L2CM: return SIGNAL_TYPE::GPS_L2CM; + case rangecmp2::SIGNAL_TYPE::GPS_L2P: return SIGNAL_TYPE::GPS_L2P; + case rangecmp2::SIGNAL_TYPE::GPS_L5Q: return SIGNAL_TYPE::GPS_L5Q; + case rangecmp2::SIGNAL_TYPE::GPS_L1C: return SIGNAL_TYPE::GPS_L1CP; + default: return SIGNAL_TYPE::UNKNOWN; + } case SATELLITE_SYSTEM::GLONASS: - return eSignalType_ == rangecmp2::SIGNAL_TYPE::GLONASS_L1CA ? SIGNAL_TYPE::GLONASS_L1CA - : eSignalType_ == rangecmp2::SIGNAL_TYPE::GLONASS_L2CA ? SIGNAL_TYPE::GLONASS_L2CA - : eSignalType_ == rangecmp2::SIGNAL_TYPE::GLONASS_L2P ? SIGNAL_TYPE::GLONASS_L2P - : eSignalType_ == rangecmp2::SIGNAL_TYPE::GLONASS_L3Q ? SIGNAL_TYPE::GLONASS_L3Q - : SIGNAL_TYPE::UNKNOWN; + switch (eSignalType_) + { + case rangecmp2::SIGNAL_TYPE::GLONASS_L1CA: return SIGNAL_TYPE::GLONASS_L1CA; + case rangecmp2::SIGNAL_TYPE::GLONASS_L2CA: return SIGNAL_TYPE::GLONASS_L2CA; + case rangecmp2::SIGNAL_TYPE::GLONASS_L2P: return SIGNAL_TYPE::GLONASS_L2P; + case rangecmp2::SIGNAL_TYPE::GLONASS_L3Q: return SIGNAL_TYPE::GLONASS_L3Q; + default: return SIGNAL_TYPE::UNKNOWN; + } case SATELLITE_SYSTEM::BEIDOU: - return eSignalType_ == rangecmp2::SIGNAL_TYPE::BEIDOU_B1D1I ? SIGNAL_TYPE::BEIDOU_B1ID1 - : eSignalType_ == rangecmp2::SIGNAL_TYPE::BEIDOU_B1D2I ? SIGNAL_TYPE::BEIDOU_B1ID2 - : eSignalType_ == rangecmp2::SIGNAL_TYPE::BEIDOU_B2D1I ? SIGNAL_TYPE::BEIDOU_B2ID1 - : eSignalType_ == rangecmp2::SIGNAL_TYPE::BEIDOU_B2D2I ? SIGNAL_TYPE::BEIDOU_B2ID2 - : eSignalType_ == rangecmp2::SIGNAL_TYPE::BEIDOU_B3D1I ? SIGNAL_TYPE::BEIDOU_B3ID1 - : eSignalType_ == rangecmp2::SIGNAL_TYPE::BEIDOU_B3D2I ? SIGNAL_TYPE::BEIDOU_B3ID2 - : eSignalType_ == rangecmp2::SIGNAL_TYPE::BEIDOU_B1CP ? SIGNAL_TYPE::BEIDOU_B1CP - : eSignalType_ == rangecmp2::SIGNAL_TYPE::BEIDOU_B2AP ? SIGNAL_TYPE::BEIDOU_B2AP - : eSignalType_ == rangecmp2::SIGNAL_TYPE::BEIDOU_B2B_I ? SIGNAL_TYPE::BEIDOU_B2BI - : SIGNAL_TYPE::UNKNOWN; + switch (eSignalType_) + { + case rangecmp2::SIGNAL_TYPE::BEIDOU_B1D1I: return SIGNAL_TYPE::BEIDOU_B1ID1; + case rangecmp2::SIGNAL_TYPE::BEIDOU_B1D2I: return SIGNAL_TYPE::BEIDOU_B1ID2; + case rangecmp2::SIGNAL_TYPE::BEIDOU_B2D1I: return SIGNAL_TYPE::BEIDOU_B2ID1; + case rangecmp2::SIGNAL_TYPE::BEIDOU_B2D2I: return SIGNAL_TYPE::BEIDOU_B2ID2; + case rangecmp2::SIGNAL_TYPE::BEIDOU_B3D1I: return SIGNAL_TYPE::BEIDOU_B3ID1; + case rangecmp2::SIGNAL_TYPE::BEIDOU_B3D2I: return SIGNAL_TYPE::BEIDOU_B3ID2; + case rangecmp2::SIGNAL_TYPE::BEIDOU_B1CP: return SIGNAL_TYPE::BEIDOU_B1CP; + case rangecmp2::SIGNAL_TYPE::BEIDOU_B2AP: return SIGNAL_TYPE::BEIDOU_B2AP; + case rangecmp2::SIGNAL_TYPE::BEIDOU_B2B_I: return SIGNAL_TYPE::BEIDOU_B2BI; + default: return SIGNAL_TYPE::UNKNOWN; + } case SATELLITE_SYSTEM::GALILEO: - return eSignalType_ == rangecmp2::SIGNAL_TYPE::GALILEO_E1C ? SIGNAL_TYPE::GALILEO_E1C - : eSignalType_ == rangecmp2::SIGNAL_TYPE::GALILEO_E5AQ ? SIGNAL_TYPE::GALILEO_E5AQ - : eSignalType_ == rangecmp2::SIGNAL_TYPE::GALILEO_E5BQ ? SIGNAL_TYPE::GALILEO_E5BQ - : eSignalType_ == rangecmp2::SIGNAL_TYPE::GALILEO_ALTBOCQ ? SIGNAL_TYPE::GALILEO_E5ALTBOCQ - : eSignalType_ == rangecmp2::SIGNAL_TYPE::GALILEO_E6C ? SIGNAL_TYPE::GALILEO_E6C - : eSignalType_ == rangecmp2::SIGNAL_TYPE::GALILEO_E6B ? SIGNAL_TYPE::GALILEO_E6B - : SIGNAL_TYPE::UNKNOWN; + switch (eSignalType_) + { + case rangecmp2::SIGNAL_TYPE::GALILEO_E1C: return SIGNAL_TYPE::GALILEO_E1C; + case rangecmp2::SIGNAL_TYPE::GALILEO_E5AQ: return SIGNAL_TYPE::GALILEO_E5AQ; + case rangecmp2::SIGNAL_TYPE::GALILEO_E5BQ: return SIGNAL_TYPE::GALILEO_E5BQ; + case rangecmp2::SIGNAL_TYPE::GALILEO_ALTBOCQ: return SIGNAL_TYPE::GALILEO_E5ALTBOCQ; + case rangecmp2::SIGNAL_TYPE::GALILEO_E6C: return SIGNAL_TYPE::GALILEO_E6C; + case rangecmp2::SIGNAL_TYPE::GALILEO_E6B: return SIGNAL_TYPE::GALILEO_E6B; + default: return SIGNAL_TYPE::UNKNOWN; + } case SATELLITE_SYSTEM::QZSS: - return eSignalType_ == rangecmp2::SIGNAL_TYPE::QZSS_L1CA ? SIGNAL_TYPE::QZSS_L1CA - : eSignalType_ == rangecmp2::SIGNAL_TYPE::QZSS_L2CM ? SIGNAL_TYPE::QZSS_L2CM - : eSignalType_ == rangecmp2::SIGNAL_TYPE::QZSS_L5Q ? SIGNAL_TYPE::QZSS_L5Q - : eSignalType_ == rangecmp2::SIGNAL_TYPE::QZSS_L1C ? SIGNAL_TYPE::QZSS_L1CP - : eSignalType_ == rangecmp2::SIGNAL_TYPE::QZSS_L6P ? SIGNAL_TYPE::QZSS_L6P - : SIGNAL_TYPE::UNKNOWN; + switch (eSignalType_) + { + case rangecmp2::SIGNAL_TYPE::QZSS_L1CA: return SIGNAL_TYPE::QZSS_L1CA; + case rangecmp2::SIGNAL_TYPE::QZSS_L2CM: return SIGNAL_TYPE::QZSS_L2CM; + case rangecmp2::SIGNAL_TYPE::QZSS_L5Q: return SIGNAL_TYPE::QZSS_L5Q; + case rangecmp2::SIGNAL_TYPE::QZSS_L1C: return SIGNAL_TYPE::QZSS_L1CP; + case rangecmp2::SIGNAL_TYPE::QZSS_L6P: return SIGNAL_TYPE::QZSS_L6P; + default: SIGNAL_TYPE::UNKNOWN; + } case SATELLITE_SYSTEM::SBAS: - return eSignalType_ == rangecmp2::SIGNAL_TYPE::SBAS_L1CA ? SIGNAL_TYPE::SBAS_L1CA - : eSignalType_ == rangecmp2::SIGNAL_TYPE::SBAS_L5I ? SIGNAL_TYPE::SBAS_L5I - : SIGNAL_TYPE::UNKNOWN; - case SATELLITE_SYSTEM::NAVIC: return eSignalType_ == rangecmp2::SIGNAL_TYPE::NAVIC_L5SPS ? SIGNAL_TYPE::NAVIC_L5SPS : SIGNAL_TYPE::UNKNOWN; - case SATELLITE_SYSTEM::OTHER: return eSignalType_ == rangecmp2::SIGNAL_TYPE::LBAND ? SIGNAL_TYPE::LBAND : SIGNAL_TYPE::UNKNOWN; + switch (eSignalType_) + { + case rangecmp2::SIGNAL_TYPE::SBAS_L1CA: return SIGNAL_TYPE::SBAS_L1CA; + case rangecmp2::SIGNAL_TYPE::SBAS_L5I: return SIGNAL_TYPE::SBAS_L5I; + default: return SIGNAL_TYPE::UNKNOWN; + } + case SATELLITE_SYSTEM::NAVIC: + switch (eSignalType_) + { + case rangecmp2::SIGNAL_TYPE::NAVIC_L5SPS: return SIGNAL_TYPE::NAVIC_L5SPS; + default: return SIGNAL_TYPE::UNKNOWN; + } + case SATELLITE_SYSTEM::OTHER: + switch (eSignalType_) + { + case rangecmp2::SIGNAL_TYPE::LBAND: return SIGNAL_TYPE::LBAND; + default: return SIGNAL_TYPE::UNKNOWN; + } default: return SIGNAL_TYPE::UNKNOWN; } } @@ -878,51 +906,74 @@ struct ChannelTrackingStatus switch (eSystem_) { case SATELLITE_SYSTEM::GPS: - return eSignalType_ == rangecmp4::SIGNAL_TYPE::GPS_L1CA ? SIGNAL_TYPE::GPS_L1CA - : eSignalType_ == rangecmp4::SIGNAL_TYPE::GPS_L2Y ? SIGNAL_TYPE::GPS_L2Y - : eSignalType_ == rangecmp4::SIGNAL_TYPE::GPS_L2C ? SIGNAL_TYPE::GPS_L2CM - : eSignalType_ == rangecmp4::SIGNAL_TYPE::GPS_L2P ? SIGNAL_TYPE::GPS_L2P - : eSignalType_ == rangecmp4::SIGNAL_TYPE::GPS_L5Q ? SIGNAL_TYPE::GPS_L5Q - : eSignalType_ == rangecmp4::SIGNAL_TYPE::GPS_L1C ? SIGNAL_TYPE::GPS_L1CP - : SIGNAL_TYPE::UNKNOWN; + switch (eSignalType_) + { + case rangecmp4::SIGNAL_TYPE::GPS_L1CA: return SIGNAL_TYPE::GPS_L1CA; + case rangecmp4::SIGNAL_TYPE::GPS_L2Y: return SIGNAL_TYPE::GPS_L2Y; + case rangecmp4::SIGNAL_TYPE::GPS_L2C: return SIGNAL_TYPE::GPS_L2CM; + case rangecmp4::SIGNAL_TYPE::GPS_L2P: return SIGNAL_TYPE::GPS_L2P; + case rangecmp4::SIGNAL_TYPE::GPS_L5Q: return SIGNAL_TYPE::GPS_L5Q; + case rangecmp4::SIGNAL_TYPE::GPS_L1C: return SIGNAL_TYPE::GPS_L1CP; + default: return SIGNAL_TYPE::UNKNOWN; + } case SATELLITE_SYSTEM::GLONASS: - return eSignalType_ == rangecmp4::SIGNAL_TYPE::GLONASS_L1CA ? SIGNAL_TYPE::GLONASS_L1CA - : eSignalType_ == rangecmp4::SIGNAL_TYPE::GLONASS_L2CA ? SIGNAL_TYPE::GLONASS_L2CA - : eSignalType_ == rangecmp4::SIGNAL_TYPE::GLONASS_L2P ? SIGNAL_TYPE::GLONASS_L2P - : eSignalType_ == rangecmp4::SIGNAL_TYPE::GLONASS_L3 ? SIGNAL_TYPE::GLONASS_L3Q - : SIGNAL_TYPE::UNKNOWN; + switch (eSignalType_) + { + case rangecmp4::SIGNAL_TYPE::GLONASS_L1CA: return SIGNAL_TYPE::GLONASS_L1CA; + case rangecmp4::SIGNAL_TYPE::GLONASS_L2CA: return SIGNAL_TYPE::GLONASS_L2CA; + case rangecmp4::SIGNAL_TYPE::GLONASS_L2P: return SIGNAL_TYPE::GLONASS_L2P; + case rangecmp4::SIGNAL_TYPE::GLONASS_L3: return SIGNAL_TYPE::GLONASS_L3Q; + default: return SIGNAL_TYPE::UNKNOWN; + } case SATELLITE_SYSTEM::BEIDOU: - return eSignalType_ == rangecmp4::SIGNAL_TYPE::BEIDOU_B1I ? SIGNAL_TYPE::BEIDOU_B1ID1 - : eSignalType_ == rangecmp4::SIGNAL_TYPE::BEIDOU_B1GEO ? SIGNAL_TYPE::BEIDOU_B1ID2 - : eSignalType_ == rangecmp4::SIGNAL_TYPE::BEIDOU_B2I ? SIGNAL_TYPE::BEIDOU_B2ID1 - : eSignalType_ == rangecmp4::SIGNAL_TYPE::BEIDOU_B2GEO ? SIGNAL_TYPE::BEIDOU_B2ID2 - : eSignalType_ == rangecmp4::SIGNAL_TYPE::BEIDOU_B3I ? SIGNAL_TYPE::BEIDOU_B3ID1 - : eSignalType_ == rangecmp4::SIGNAL_TYPE::BEIDOU_B3GEO ? SIGNAL_TYPE::BEIDOU_B3ID2 - : eSignalType_ == rangecmp4::SIGNAL_TYPE::BEIDOU_B1CP ? SIGNAL_TYPE::BEIDOU_B1CP - : eSignalType_ == rangecmp4::SIGNAL_TYPE::BEIDOU_B2AP ? SIGNAL_TYPE::BEIDOU_B2AP - : eSignalType_ == rangecmp4::SIGNAL_TYPE::BEIDOU_B2BI ? SIGNAL_TYPE::BEIDOU_B2BI - : SIGNAL_TYPE::UNKNOWN; + switch (eSignalType_) + { + case rangecmp4::SIGNAL_TYPE::BEIDOU_B1I: return SIGNAL_TYPE::BEIDOU_B1ID1; + case rangecmp4::SIGNAL_TYPE::BEIDOU_B1GEO: return SIGNAL_TYPE::BEIDOU_B1ID2; + case rangecmp4::SIGNAL_TYPE::BEIDOU_B2I: return SIGNAL_TYPE::BEIDOU_B2ID1; + case rangecmp4::SIGNAL_TYPE::BEIDOU_B2GEO: return SIGNAL_TYPE::BEIDOU_B2ID2; + case rangecmp4::SIGNAL_TYPE::BEIDOU_B3I: return SIGNAL_TYPE::BEIDOU_B3ID1; + case rangecmp4::SIGNAL_TYPE::BEIDOU_B3GEO: return SIGNAL_TYPE::BEIDOU_B3ID2; + case rangecmp4::SIGNAL_TYPE::BEIDOU_B1CP: return SIGNAL_TYPE::BEIDOU_B1CP; + case rangecmp4::SIGNAL_TYPE::BEIDOU_B2AP: return SIGNAL_TYPE::BEIDOU_B2AP; + case rangecmp4::SIGNAL_TYPE::BEIDOU_B2BI: return SIGNAL_TYPE::BEIDOU_B2BI; + default: return SIGNAL_TYPE::UNKNOWN; + } case SATELLITE_SYSTEM::GALILEO: - return eSignalType_ == rangecmp4::SIGNAL_TYPE::GALILEO_E1 ? SIGNAL_TYPE::GALILEO_E1C - : eSignalType_ == rangecmp4::SIGNAL_TYPE::GALILEO_E5A ? SIGNAL_TYPE::GALILEO_E5AQ - : eSignalType_ == rangecmp4::SIGNAL_TYPE::GALILEO_E5B ? SIGNAL_TYPE::GALILEO_E5BQ - : eSignalType_ == rangecmp4::SIGNAL_TYPE::GALILEO_ALTBOC ? SIGNAL_TYPE::GALILEO_E5ALTBOCQ - : eSignalType_ == rangecmp4::SIGNAL_TYPE::GALILEO_E6C ? SIGNAL_TYPE::GALILEO_E6C - : eSignalType_ == rangecmp4::SIGNAL_TYPE::GALILEO_E6B ? SIGNAL_TYPE::GALILEO_E6B - : SIGNAL_TYPE::UNKNOWN; + switch (eSignalType_) + { + case rangecmp4::SIGNAL_TYPE::GALILEO_E1: return SIGNAL_TYPE::GALILEO_E1C; + case rangecmp4::SIGNAL_TYPE::GALILEO_E5A: return SIGNAL_TYPE::GALILEO_E5AQ; + case rangecmp4::SIGNAL_TYPE::GALILEO_E5B: return SIGNAL_TYPE::GALILEO_E5BQ; + case rangecmp4::SIGNAL_TYPE::GALILEO_ALTBOC: return SIGNAL_TYPE::GALILEO_E5ALTBOCQ; + case rangecmp4::SIGNAL_TYPE::GALILEO_E6C: return SIGNAL_TYPE::GALILEO_E6C; + case rangecmp4::SIGNAL_TYPE::GALILEO_E6B: return SIGNAL_TYPE::GALILEO_E6B; + default: return SIGNAL_TYPE::UNKNOWN; + } case SATELLITE_SYSTEM::QZSS: - return eSignalType_ == rangecmp4::SIGNAL_TYPE::QZSS_L1CA ? SIGNAL_TYPE::QZSS_L1CA - : eSignalType_ == rangecmp4::SIGNAL_TYPE::QZSS_L2C ? SIGNAL_TYPE::QZSS_L2CM - : eSignalType_ == rangecmp4::SIGNAL_TYPE::QZSS_L5Q ? SIGNAL_TYPE::QZSS_L5Q - : eSignalType_ == rangecmp4::SIGNAL_TYPE::QZSS_L1C ? SIGNAL_TYPE::QZSS_L1CP - : eSignalType_ == rangecmp4::SIGNAL_TYPE::QZSS_L6D ? SIGNAL_TYPE::QZSS_L6D - : eSignalType_ == rangecmp4::SIGNAL_TYPE::QZSS_L6P ? SIGNAL_TYPE::QZSS_L6P - : SIGNAL_TYPE::UNKNOWN; + switch (eSignalType_) + { + case rangecmp4::SIGNAL_TYPE::QZSS_L1CA: return SIGNAL_TYPE::QZSS_L1CA; + case rangecmp4::SIGNAL_TYPE::QZSS_L2C: return SIGNAL_TYPE::QZSS_L2CM; + case rangecmp4::SIGNAL_TYPE::QZSS_L5Q: return SIGNAL_TYPE::QZSS_L5Q; + case rangecmp4::SIGNAL_TYPE::QZSS_L1C: return SIGNAL_TYPE::QZSS_L1CP; + case rangecmp4::SIGNAL_TYPE::QZSS_L6D: return SIGNAL_TYPE::QZSS_L6D; + case rangecmp4::SIGNAL_TYPE::QZSS_L6P: return SIGNAL_TYPE::QZSS_L6P; + default: return SIGNAL_TYPE::UNKNOWN; + } case SATELLITE_SYSTEM::SBAS: - return eSignalType_ == rangecmp4::SIGNAL_TYPE::SBAS_L1CA ? SIGNAL_TYPE::SBAS_L1CA - : eSignalType_ == rangecmp4::SIGNAL_TYPE::SBAS_L5I ? SIGNAL_TYPE::SBAS_L5I - : SIGNAL_TYPE::UNKNOWN; - case SATELLITE_SYSTEM::NAVIC: return eSignalType_ == rangecmp4::SIGNAL_TYPE::NAVIC_L5SPS ? SIGNAL_TYPE::NAVIC_L5SPS : SIGNAL_TYPE::UNKNOWN; + switch (eSignalType_) + { + case rangecmp4::SIGNAL_TYPE::SBAS_L1CA: return SIGNAL_TYPE::SBAS_L1CA; + case rangecmp4::SIGNAL_TYPE::SBAS_L5I: return SIGNAL_TYPE::SBAS_L5I; + default: return SIGNAL_TYPE::UNKNOWN; + } + case SATELLITE_SYSTEM::NAVIC: + switch (eSignalType_) + { + case rangecmp4::SIGNAL_TYPE::NAVIC_L5SPS: return SIGNAL_TYPE::NAVIC_L5SPS; + default: return SIGNAL_TYPE::UNKNOWN; + } default: return SIGNAL_TYPE::UNKNOWN; } } @@ -930,14 +981,17 @@ struct ChannelTrackingStatus //! Convert a SYSTEM enumeration to a channel tracking status SATELLITE_SYSTEM. static SATELLITE_SYSTEM SystemToSatelliteSystem(SYSTEM eSystem_) { - return eSystem_ == SYSTEM::GPS ? SATELLITE_SYSTEM::GPS - : eSystem_ == SYSTEM::GLONASS ? SATELLITE_SYSTEM::GLONASS - : eSystem_ == SYSTEM::SBAS ? SATELLITE_SYSTEM::SBAS - : eSystem_ == SYSTEM::GALILEO ? SATELLITE_SYSTEM::GALILEO - : eSystem_ == SYSTEM::BEIDOU ? SATELLITE_SYSTEM::BEIDOU - : eSystem_ == SYSTEM::QZSS ? SATELLITE_SYSTEM::QZSS - : eSystem_ == SYSTEM::NAVIC ? SATELLITE_SYSTEM::NAVIC - : SATELLITE_SYSTEM::OTHER; + switch (eSystem_) + { + case SYSTEM::GPS: return SATELLITE_SYSTEM::GPS; + case SYSTEM::GLONASS: return SATELLITE_SYSTEM::GLONASS; + case SYSTEM::SBAS: return SATELLITE_SYSTEM::SBAS; + case SYSTEM::GALILEO: return SATELLITE_SYSTEM::GALILEO; + case SYSTEM::BEIDOU: return SATELLITE_SYSTEM::BEIDOU; + case SYSTEM::QZSS: return SATELLITE_SYSTEM::QZSS; + case SYSTEM::NAVIC: return SATELLITE_SYSTEM::NAVIC; + default: return SATELLITE_SYSTEM::OTHER; + } } //! Combine the channel tracking status fields into a single 4-byte value according to documentation: diff --git a/src/decoders/oem/src/rangecmp/range_decompressor.cpp b/src/decoders/oem/src/rangecmp/range_decompressor.cpp index 29ad1fcd4..3ce51c76d 100644 --- a/src/decoders/oem/src/rangecmp/range_decompressor.cpp +++ b/src/decoders/oem/src/rangecmp/range_decompressor.cpp @@ -142,54 +142,74 @@ double RangeDecompressor::GetSignalWavelength(const ChannelTrackingStatus& stCha switch (stChannelStatus_.eSatelliteSystem) { case System::GPS: - return stChannelStatus_.eSignalType == Signal::GPS_L1CA ? WAVELENGTH_GPS_L1 - : stChannelStatus_.eSignalType == Signal::GPS_L1CP ? WAVELENGTH_GPS_L1 - : stChannelStatus_.eSignalType == Signal::GPS_L2P ? WAVELENGTH_GPS_L2 - : stChannelStatus_.eSignalType == Signal::GPS_L2Y ? WAVELENGTH_GPS_L2 - : stChannelStatus_.eSignalType == Signal::GPS_L2CM ? WAVELENGTH_GPS_L2 - : stChannelStatus_.eSignalType == Signal::GPS_L5Q ? WAVELENGTH_GPS_L5 - : 0.0; + switch (stChannelStatus_.eSignalType) + { + case Signal::GPS_L1CA: return WAVELENGTH_GPS_L1; + case Signal::GPS_L1CP: return WAVELENGTH_GPS_L1; + case Signal::GPS_L2P: return WAVELENGTH_GPS_L2; + case Signal::GPS_L2Y: return WAVELENGTH_GPS_L2; + case Signal::GPS_L2CM: return WAVELENGTH_GPS_L2; + case Signal::GPS_L5Q: return WAVELENGTH_GPS_L5; + default: return 0.0; + } case System::GLONASS: - return stChannelStatus_.eSignalType == Signal::GLONASS_L1CA - ? SPEED_OF_LIGHT / (FREQUENCY_HZ_GLO_L1 + sGLONASSFrequency_ * GLONASS_L1_FREQUENCY_SCALE_HZ) - : stChannelStatus_.eSignalType == Signal::GLONASS_L2CA - ? SPEED_OF_LIGHT / (FREQUENCY_HZ_GLO_L2 + sGLONASSFrequency_ * GLONASS_L2_FREQUENCY_SCALE_HZ) - : stChannelStatus_.eSignalType == Signal::GLONASS_L2P - ? SPEED_OF_LIGHT / (FREQUENCY_HZ_GLO_L2 + sGLONASSFrequency_ * GLONASS_L2_FREQUENCY_SCALE_HZ) - : stChannelStatus_.eSignalType == Signal::GLONASS_L3Q ? WAVELENGTH_GLO_L3 - : 0.0; + switch (stChannelStatus_.eSignalType) + { + case Signal::GLONASS_L1CA: return SPEED_OF_LIGHT / (FREQUENCY_HZ_GLO_L1 + sGLONASSFrequency_ * GLONASS_L1_FREQUENCY_SCALE_HZ); + case Signal::GLONASS_L2CA: return SPEED_OF_LIGHT / (FREQUENCY_HZ_GLO_L2 + sGLONASSFrequency_ * GLONASS_L2_FREQUENCY_SCALE_HZ); + case Signal::GLONASS_L2P: return SPEED_OF_LIGHT / (FREQUENCY_HZ_GLO_L2 + sGLONASSFrequency_ * GLONASS_L2_FREQUENCY_SCALE_HZ); + case Signal::GLONASS_L3Q: return WAVELENGTH_GLO_L3; + default: return 0.0; + } case System::SBAS: - return stChannelStatus_.eSignalType == Signal::SBAS_L1CA ? WAVELENGTH_GPS_L1 - : stChannelStatus_.eSignalType == Signal::SBAS_L5I ? WAVELENGTH_GPS_L5 - : 0.0; + switch (stChannelStatus_.eSignalType) + { + case Signal::SBAS_L1CA: return WAVELENGTH_GPS_L1; + case Signal::SBAS_L5I: return WAVELENGTH_GPS_L5; + default: return 0.0; + } case System::GALILEO: - return stChannelStatus_.eSignalType == Signal::GALILEO_E1C ? WAVELENGTH_GAL_E1 - : stChannelStatus_.eSignalType == Signal::GALILEO_E6B ? WAVELENGTH_GAL_E6 - : stChannelStatus_.eSignalType == Signal::GALILEO_E6C ? WAVELENGTH_GAL_E6 - : stChannelStatus_.eSignalType == Signal::GALILEO_E5AQ ? WAVELENGTH_GAL_E5AQ - : stChannelStatus_.eSignalType == Signal::GALILEO_E5BQ ? WAVELENGTH_GAL_E5BQ - : stChannelStatus_.eSignalType == Signal::GALILEO_E5ALTBOCQ ? WAVELENGTH_GAL_ALTBQ - : 0.0; + switch (stChannelStatus_.eSignalType) + { + case Signal::GALILEO_E1C: return WAVELENGTH_GAL_E1; + case Signal::GALILEO_E6B: return WAVELENGTH_GAL_E6; + case Signal::GALILEO_E6C: return WAVELENGTH_GAL_E6; + case Signal::GALILEO_E5AQ: return WAVELENGTH_GAL_E5AQ; + case Signal::GALILEO_E5BQ: return WAVELENGTH_GAL_E5BQ; + case Signal::GALILEO_E5ALTBOCQ: return WAVELENGTH_GAL_ALTBQ; + default: return 0.0; + } case System::BEIDOU: - return stChannelStatus_.eSignalType == Signal::BEIDOU_B1ID1 ? WAVELENGTH_BDS_B1 - : stChannelStatus_.eSignalType == Signal::BEIDOU_B1ID2 ? WAVELENGTH_BDS_B1 - : stChannelStatus_.eSignalType == Signal::BEIDOU_B2ID1 ? WAVELENGTH_BDS_B2 - : stChannelStatus_.eSignalType == Signal::BEIDOU_B2ID2 ? WAVELENGTH_BDS_B2 - : stChannelStatus_.eSignalType == Signal::BEIDOU_B3ID1 ? WAVELENGTH_BDS_B3 - : stChannelStatus_.eSignalType == Signal::BEIDOU_B3ID2 ? WAVELENGTH_BDS_B3 - : stChannelStatus_.eSignalType == Signal::BEIDOU_B1CP ? WAVELENGTH_BDS_B1C - : stChannelStatus_.eSignalType == Signal::BEIDOU_B2AP ? WAVELENGTH_BDS_B2A - : stChannelStatus_.eSignalType == Signal::BEIDOU_B2BI ? WAVELENGTH_BDS_B2B - : 0.0; + switch (stChannelStatus_.eSignalType) + { + case Signal::BEIDOU_B1ID1: return WAVELENGTH_BDS_B1; + case Signal::BEIDOU_B1ID2: return WAVELENGTH_BDS_B1; + case Signal::BEIDOU_B2ID1: return WAVELENGTH_BDS_B2; + case Signal::BEIDOU_B2ID2: return WAVELENGTH_BDS_B2; + case Signal::BEIDOU_B3ID1: return WAVELENGTH_BDS_B3; + case Signal::BEIDOU_B3ID2: return WAVELENGTH_BDS_B3; + case Signal::BEIDOU_B1CP: return WAVELENGTH_BDS_B1C; + case Signal::BEIDOU_B2AP: return WAVELENGTH_BDS_B2A; + case Signal::BEIDOU_B2BI: return WAVELENGTH_BDS_B2B; + default: return 0.0; + } case System::QZSS: - return stChannelStatus_.eSignalType == Signal::QZSS_L1CA ? WAVELENGTH_QZSS_L1 - : stChannelStatus_.eSignalType == Signal::QZSS_L1CP ? WAVELENGTH_QZSS_L1 - : stChannelStatus_.eSignalType == Signal::QZSS_L2CM ? WAVELENGTH_QZSS_L2 - : stChannelStatus_.eSignalType == Signal::QZSS_L5Q ? WAVELENGTH_QZSS_L5 - : stChannelStatus_.eSignalType == Signal::QZSS_L6P ? WAVELENGTH_QZSS_L6 - : stChannelStatus_.eSignalType == Signal::QZSS_L6D ? WAVELENGTH_QZSS_L6 - : 0.0; - case System::NAVIC: return stChannelStatus_.eSignalType == Signal::NAVIC_L5SPS ? WAVELENGTH_NAVIC_L5 : 0.0; + switch (stChannelStatus_.eSignalType) + { + case Signal::QZSS_L1CA: return WAVELENGTH_QZSS_L1; + case Signal::QZSS_L1CP: return WAVELENGTH_QZSS_L1; + case Signal::QZSS_L2CM: return WAVELENGTH_QZSS_L2; + case Signal::QZSS_L5Q: return WAVELENGTH_QZSS_L5; + case Signal::QZSS_L6P: return WAVELENGTH_QZSS_L6; + case Signal::QZSS_L6D: return WAVELENGTH_QZSS_L6; + default: 0.0; + } + case System::NAVIC: + switch (stChannelStatus_.eSignalType) + { + case Signal::NAVIC_L5SPS: return WAVELENGTH_NAVIC_L5; + default: return 0.0; + } default: return 0.0; } }