Skip to content

Add more SBF types #178

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
flipflip8952 merged 1 commit into from
Dec 31, 2025
+112 −31
Merged

Add more SBF types #178

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
114 changes: 83 additions & 31 deletions fpsdk_common/include/fpsdk_common/parser/sbf.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,10 @@
*
* The information on message structures, IDs, descriptions etc. in this file are from publicly available data, such as:
* - mosaic-G5 Reference Guide, copyright 2000-2025 Septentrio NV/SA, part of HEXAGON
* (direct download link from https://www.ardusimple.com/how-to-configure-septentrio-mosaic-g5/#documentation)
* - sbf2asc source code, (c) Copyright 2002-2015 Septentrio NV/SA
* - https://github.com/septentrio-gnss/septentrio_gnss_driver
* - https://github.com/rtklibexplorer/RTKLIB
* \endverbatim
*
* @file
Expand Down Expand Up @@ -712,12 +716,12 @@ static_assert(sizeof(MeasEpochChannelType1Rev0) == 20, "");
*/
struct MeasEpochChannelType2Rev0
{ // clang-format off
uint8_t Type; //!< -> MEASEPOCH_CHANNEL_TYPE_...
uint8_t Type; //!< -> SBF_MEASEPOCH_CHANNEL_TYPE_...
uint8_t LockTime; //!< Lock time [s]
uint8_t CN0; //!< C/N0 [0.25dBHz]
uint8_t OffsetsMSB; //!< @todo document
int8_t CarrierMSB; //!< @todo document
uint8_t ObsInfo; //!< -> MEASEPOCH_CHANNEL_OBSINFO_...
uint8_t ObsInfo; //!< -> SBF_MEASEPOCH_CHANNEL_OBSINFO_...
uint16_t CodeOffsetLSB; //!< @todo document
uint16_t CarrierLSB; //!< @todo document
uint16_t DopplerOffsetLSB; //!< @todo document
Expand All @@ -730,12 +734,12 @@ static constexpr std::size_t SBF_MEASEPOCH_REV1_MIN_SIZE = sizeof(MeasEpochRev1)
// clang-format on

// clang-format off
static constexpr uint8_t MEASEPOCH_CHANNEL_TYPE_SIGIDXLO(const uint8_t type) { return type & 0x1f; } //!< Signal number (part 1)
static constexpr uint8_t MEASEPOCH_CHANNEL_TYPE_ANTID(const uint8_t type) { return (type >> 5) & 0x07; } //!< Antenna ID
static constexpr uint8_t MEASEPOCH_CHANNEL_TYPE_ANTID_MAIN = 0; //!< Main antenna
static constexpr uint8_t MEASEPOCH_CHANNEL_TYPE_ANTID_AUX1 = 1; //!< Aux1 antenna
static constexpr uint8_t MEASEPOCH_CHANNEL_TYPE_ANTID_AUX2 = 2; //!< Aux2 antenna
static constexpr uint8_t MEASEPOCH_CHANNEL_OBSINFO_SIGIDXHI(const uint8_t info){ return (info >> 3) & 0x1f; } //!< Signal number (part 2)
static constexpr uint8_t SBF_MEASEPOCH_CHANNEL_TYPE_SIGIDXLO(const uint8_t type) { return type & 0x1f; } //!< Signal number (part 1)
static constexpr uint8_t SBF_MEASEPOCH_CHANNEL_TYPE_ANTID(const uint8_t type) { return (type >> 5) & 0x07; } //!< Antenna ID
static constexpr uint8_t SBF_MEASEPOCH_CHANNEL_TYPE_ANTID_MAIN = 0; //!< Main antenna
static constexpr uint8_t SBF_MEASEPOCH_CHANNEL_TYPE_ANTID_AUX1 = 1; //!< Aux1 antenna
static constexpr uint8_t SBF_MEASEPOCH_CHANNEL_TYPE_ANTID_AUX2 = 2; //!< Aux2 antenna
static constexpr uint8_t SBF_MEASEPOCH_CHANNEL_OBSINFO_SIGIDXHI(const uint8_t info){ return (info >> 3) & 0x1f; } //!< Signal number (part 2)
// clang-format on

// ---------------------------------------------------------------------------------------------------------------------
Expand Down Expand Up @@ -763,8 +767,8 @@ struct SFB_PACKED ChannelSatInfoRev0
uint8_t SVID; //!< Satellite ID
uint8_t FreqNr; //!< GLONASS frequency number
uint16_t SVIDFull; //!< If the SVID field is zero, this field contains the satellite ID
uint16_t AzimuthRiseSet; //!< -> CHANNELSTATUS_SI_AZRS_...
uint16_t HealthStatus; //!< -> CHANNELSTATUS_SI_HEALTH_...
uint16_t AzimuthRiseSet; //!< -> SBF_CHANNELSTATUS_SI_AZRS_...
uint16_t HealthStatus; //!< -> SBF_CHANNELSTATUS_SI_HEALTH_...
int8_t Elevation; //!< Elevation [deg]
uint8_t N2; //!< Number of ChannelStateInfo blocks following this ChannelSatInfo block.
uint8_t RxChannel; //!< Channel number
Expand All @@ -780,8 +784,8 @@ struct SFB_PACKED ChannelStateInfoRev0
{ // clang-format off
uint8_t Antenna; //!< Anntena number (0 = main antenna)
uint8_t Reserved; //!< Reserved
uint16_t TrackingStatus; //!< -> CHANNELSTATE_TRKSTA_...
uint16_t PVTStatus; //!< -> CHANNELSTATE_PVTSTA_...
uint16_t TrackingStatus; //!< -> SBF_CHANNELSTATE_TRKSTA_...
uint16_t PVTStatus; //!< -> SBF_CHANNELSTATE_PVTSTA_...
uint16_t PVTInfo; //!< Internal info
}; // clang-format on

Expand All @@ -792,25 +796,25 @@ static constexpr std::size_t SBF_CHANNELSTATUS_REV0_MIN_SIZE = sizeof(ChannelSta
// clang-format on

// clang-format off
static constexpr uint16_t CHANNELSTATUS_SI_AZRS_AZIMUTH(const uint16_t azrs) { return azrs & 0x1ff; } //!< Azimuth [deg]
static constexpr uint8_t CHANNELSTATUS_SI_AZRS_RISESET(const uint16_t azrs) { return (azrs >> 14) & 0x0007; } //!< Rise/set indicator
static constexpr uint8_t CHANNELSTATUS_SI_AZRS_RISESET_SETTING = 0; //!< SV is setting
static constexpr uint8_t CHANNELSTATUS_SI_AZRS_RISESET_RISING = 1; //!< SV is rising
static constexpr uint8_t CHANNELSTATUS_SI_AZRS_RISESET_UNKNOWN = 2; //!< Elevation rate unknown
static constexpr uint8_t CHANNELSTATUS_SI_HEALTH_SIG(const uint16_t health, const std::size_t sigIx) { return (health >> (sigIx * 2)) & 0x07; } //!< Signal health
static constexpr uint8_t CHANNELSTATUS_SI_HEALTH_SIG_UNKNOWN = 0; //!< Health unknown
static constexpr uint8_t CHANNELSTATUS_SI_HEALTH_SIG_HEALTHY = 1; //!< Healthy
static constexpr uint8_t CHANNELSTATUS_SI_HEALTH_SIG_UNHEALTHY = 2; //!< Unhealthy
static constexpr uint8_t CHANNELSTATE_TRKSTA_SIG(const uint16_t status, const std::size_t sigIx) { return (status >> (sigIx * 2)) & 0x07; } //!< Signal health
static constexpr uint8_t CHANNELSTATE_TRKSTA_SIG_IDLE = 0; //!< Idle
static constexpr uint8_t CHANNELSTATE_TRKSTA_SIG_SEARCH = 1; //!< Search
static constexpr uint8_t CHANNELSTATE_TRKSTA_SIG_SYNC = 2; //!< Sync
static constexpr uint8_t CHANNELSTATE_TRKSTA_SIG_TRACKING = 3; //!< Tracking
static constexpr uint8_t CHANNELSTATE_PVTSTA_SIG(const uint16_t status, const std::size_t sigIx) { return (status >> (sigIx * 2)) & 0x07; } //!< Signal health
static constexpr uint8_t CHANNELSTATE_PVTSTA_SIG_UNUSED = 0; //!< Not used
static constexpr uint8_t CHANNELSTATE_PVTSTA_SIG_NOEPH = 1; //!< No ephemeris
static constexpr uint8_t CHANNELSTATE_PVTSTA_SIG_USED = 2; //!< Used
static constexpr uint8_t CHANNELSTATE_PVTSTA_SIG_REJECTED = 3; //!< Rejected
static constexpr uint16_t SBF_CHANNELSTATUS_SI_AZRS_AZIMUTH(const uint16_t azrs){ return azrs & 0x1ff; } //!< Azimuth [deg]
static constexpr uint8_t SBF_CHANNELSTATUS_SI_AZRS_RISESET(const uint16_t azrs){ return (azrs >> 14) & 0x0007; } //!< Rise/set indicator
static constexpr uint8_t SBF_CHANNELSTATUS_SI_AZRS_RISESET_SETTING = 0; //!< SV is setting
static constexpr uint8_t SBF_CHANNELSTATUS_SI_AZRS_RISESET_RISING = 1; //!< SV is rising
static constexpr uint8_t SBF_CHANNELSTATUS_SI_AZRS_RISESET_UNKNOWN = 2; //!< Elevation rate unknown
static constexpr uint8_t SBF_CHANNELSTATUS_SI_HEALTH_SIG(const uint16_t health, const std::size_t sigIx) { return (health >> (sigIx * 2)) & 0x07; } //!< Signal health
static constexpr uint8_t SBF_CHANNELSTATUS_SI_HEALTH_SIG_UNKNOWN = 0; //!< Health unknown
static constexpr uint8_t SBF_CHANNELSTATUS_SI_HEALTH_SIG_HEALTHY = 1; //!< Healthy
static constexpr uint8_t SBF_CHANNELSTATUS_SI_HEALTH_SIG_UNHEALTHY = 2; //!< Unhealthy
static constexpr uint8_t SBF_CHANNELSTATE_TRKSTA_SIG(const uint16_t status, const std::size_t sigIx) { return (status >> (sigIx * 2)) & 0x07; } //!< Signal health
static constexpr uint8_t SBF_CHANNELSTATE_TRKSTA_SIG_IDLE = 0; //!< Idle
static constexpr uint8_t SBF_CHANNELSTATE_TRKSTA_SIG_SEARCH = 1; //!< Search
static constexpr uint8_t SBF_CHANNELSTATE_TRKSTA_SIG_SYNC = 2; //!< Sync
static constexpr uint8_t SBF_CHANNELSTATE_TRKSTA_SIG_TRACKING = 3; //!< Tracking
static constexpr uint8_t SBF_CHANNELSTATE_PVTSTA_SIG(const uint16_t status, const std::size_t sigIx) { return (status >> (sigIx * 2)) & 0x07; } //!< Signal health
static constexpr uint8_t SBF_CHANNELSTATE_PVTSTA_SIG_UNUSED = 0; //!< Not used
static constexpr uint8_t SBF_CHANNELSTATE_PVTSTA_SIG_NOEPH = 1; //!< No ephemeris
static constexpr uint8_t SBF_CHANNELSTATE_PVTSTA_SIG_USED = 2; //!< Used
static constexpr uint8_t SBF_CHANNELSTATE_PVTSTA_SIG_REJECTED = 3; //!< Rejected
// clang-format on

// ---------------------------------------------------------------------------------------------------------------------
Expand All @@ -831,6 +835,54 @@ static_assert(sizeof(SbfEndOfAnyRev0) == 8, "");
static constexpr std::size_t SBF_ENDOFANY_REV0_SIZE = sizeof(SbfEndOfAnyRev0) + SBF_HEAD_SIZE; //!< Size of EndOfPvt/EndOfAtt/EndOfMeas message
// clang-format on

// ---------------------------------------------------------------------------------------------------------------------

/**
* @brief SBF BBSamples payload head (rev 0)
*/
struct SbfBBSamplesHeadRev0
{ // clang-format off
uint32_t TOW; //!< GPS time of week [ms]
uint16_t WNc; //!< GPS week number
uint16_t N; //!< Number of samples
uint8_t info; //!< -> SBF_BBSAMPLES_INFO_...
uint8_t reserved[3]; //!< Reserved
uint32_t SampleFreq; //!< Sampling frequency [Hz] FIXME: looks like this is the span (bandwidth)
uint32_t LOFreq; //!< Local oscillator frequency [Hz] FIXME: looks like this is the centre frequency
}; // clang-format on

/**
* @brief SBF BBSamples payload sample (rev 0)
*/
struct SbfBBSamplesSampleRev0
{ // clang-format off
int8_t I; //!< I component
int8_t Q; //!< Q component
}; // clang-format on

static_assert(sizeof(SbfBBSamplesSampleRev0) == 2, "");

/**
* @brief SBF BBSamples payload tail (rev 0)
*/
struct SbfBBSamplesTailRev0
{ // clang-format off
float TOWDelta; //!< Time offset [s]
}; // clang-format on

static_assert(sizeof(SbfBBSamplesTailRev0) == 4, "");

// clang-format off
static constexpr std::size_t SBF_BBSAMPLES_REV0_MIN_SIZE = sizeof(SbfBBSamplesHeadRev0) + sizeof(SbfBBSamplesTailRev0) + SBF_HEAD_SIZE; //!< Minimal size of BBSamples message
// clang-format on

// clang-format off
static constexpr uint8_t SBF_BBSAMPLES_INFO_ANTID(const uint8_t type) { return type & 0x07; } //!< Antenna ID
static constexpr uint8_t SBF_BBSAMPLES_INFO_ANTID_MAIN = 0; //!< Main antenna
static constexpr uint8_t SBF_BBSAMPLES_INFO_ANTID_AUX1 = 1; //!< Aux1 antenna
static constexpr uint8_t SBF_BBSAMPLES_INFO_ANTID_AUX2 = 2; //!< Aux2 antenna
// clang-format on

/* ****************************************************************************************************************** */
} // namespace sbf
} // namespace parser
Expand Down
29 changes: 29 additions & 0 deletions fpsdk_common/src/parser/sbf.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -241,6 +241,34 @@ static std::size_t StrPvtCartesian(char* info, const std::size_t size, const uin
return len;
}

static std::size_t StrBbSamples(char* info, const std::size_t size, const uint8_t* msg, const std::size_t msg_size)
{
using namespace fpsdk::common::math;

if ((msg_size < SBF_BBSAMPLES_REV0_MIN_SIZE) || (SbfBlockRev(msg) != 0)) {
return 0;
}
std::size_t len = StrWnoTow(info, size, &msg[SBF_HEAD_SIZE]);

SbfBBSamplesHeadRev0 head;
std::memcpy(&head, &msg[SBF_HEAD_SIZE], sizeof(head));
const std::size_t exp_size =
SBF_BBSAMPLES_REV0_MIN_SIZE + (head.N * sizeof(SbfBBSamplesSampleRev0)) + sizeof(SbfBBSamplesTailRev0);
if (msg_size < exp_size) {
return len;
}
SbfBBSamplesTailRev0 tail;
std::memcpy(&tail, &msg[exp_size - sizeof(tail)], sizeof(tail));

std::array<const char*, 3> antIdStrs = { { "MAIN", "AUX1", "AUX2" } };
const std::size_t antId = SBF_BBSAMPLES_INFO_ANTID(head.info);
len += std::snprintf(&info[len], size - len, " %s %.6f %.6f %" PRIu16 " %g",
antId < antIdStrs.size() ? antIdStrs[antId] : "ANT?", (double)head.SampleFreq * 1e-6,
(double)head.LOFreq * 1e-6, head.N, tail.TOWDelta);

return len;
}

bool SbfGetMessageInfo(char* info, const std::size_t size, const uint8_t* msg, const std::size_t msg_size)
{
if ((info == NULL) || (size < 1) || (msg == NULL) || (msg_size < SBF_HEAD_SIZE)) {
Expand All @@ -262,6 +290,7 @@ bool SbfGetMessageInfo(char* info, const std::size_t size, const uint8_t* msg, c
switch (block) { // clang-format off
case SBF_PVTGEODETIC_MSGID: len = StrPvtGeodetic(info, size, msg, msg_size); break;
case SBF_PVTCARTESIAN_MSGID: len = StrPvtCartesian(info, size, msg, msg_size); break;
case SBF_BBSAMPLES_MSGID: len = StrBbSamples(info, size, msg, msg_size); break;
case SBF_ENDOFMEAS_MSGID: /* FALLTHROUGH */
case SBF_ENDOFATT_MSGID: /* FALLTHROUGH */
case SBF_ENDOFPVT_MSGID: len = StrEndOfAny(info, size, msg, msg_size); break;
Expand Down
Loading