Skip to content

Fix RTCM3 MSM header decoding, improve MSM stringification #176

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 22, 2025
Merged

Fix RTCM3 MSM header decoding, improve MSM stringification #176

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
3 changes: 2 additions & 1 deletion .github/workflows/ci.yml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -115,11 +115,12 @@ jobs:
cd ${GITHUB_WORKSPACE}/docker
COMPOSE_PARALLEL_LIMIT=1 docker compose build build-trixie-run build-noetic-run build-humble-run build-jazzy-run
- name: Push
if: ${{ github.ref == 'refs/heads/main' || github.ref == 'refs/pull/115/merge' }}
if: ${{ github.ref == 'refs/heads/main' }}
run: |
cd ${GITHUB_WORKSPACE}/docker
docker compose push build-trixie-run build-noetic-run build-humble-run build-jazzy-run
- name: Expire old images
if: ${{ github.ref == 'refs/heads/main' }}
uses: actions/delete-package-versions@v5
with:
package-name: ${{ github.event.repository.name }}
Expand Down
26 changes: 22 additions & 4 deletions fpsdk_common/include/fpsdk_common/parser/rtcm3.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -237,6 +237,15 @@ enum class Rtcm3MsmGnss : uint16_t
NAVIC = 130, //!< NavIC
}; // clang-format on

/**
* @brief Stringify RTCM3 MSM GNSS type
*
* @param[in] gnss The GNSS
*
* @returns a concise and unique string for the GNSS type, "?" for bad value
*/
const char* Rtcm3MsmGnssStr(const Rtcm3MsmGnss gnss);

/**
* @brief RTCM3 MSM messages type
*/
Expand All @@ -251,6 +260,15 @@ enum class Rtcm3MsmType : uint16_t
MSM7 = 7, //!< Type 7 (ext full C, ext full L, S, D)
};

/**
* @brief Stringify RTCM3 MSM type
*
* @param[in] msm The MSM type
*
* @returns a concise and unique string for the MSM type, "?" for bad value
*/
const char* Rtcm3MsmTypeStr(const Rtcm3MsmType gnss);

/**
* @brief RTMC3 message type to MSM GNSS and type
*
Expand Down Expand Up @@ -289,12 +307,12 @@ struct Rtcm3MsmHeader
bool smooth_; //!< GNSS divergence-free smoothing indicator (DF417, bit(1))
uint8_t smooth_int_; //!< GNSS smoothing interval (DF418, bit(3))
uint64_t sat_mask_; //!< GNSS satellite mask (DF394, bit(64))
uint64_t sig_mask_; //!< GNSS signal mask (DF395, bit(64))
uint32_t sig_mask_; //!< GNSS signal mask (DF395, bit(32))
uint64_t cell_mask_; //!< GNSS cell mask (DF396, bit(64))

int num_sat_; //!< Number of satellites (in satMask)
int num_sig_; //!< Number of signals (in sigMask)
int num_cell_; //!< Number of cells (in cellMask)
int num_sat_; //!< Number of satellites (1-bits in sat_mask_), "Nsat"
int num_sig_; //!< Number of signals (1-bits in sig_mask_), "Nsig"
int num_cell_; //!< Number of cells (1-bits in cell_mask_), "Ncell"
};

/**
Expand Down
59 changes: 52 additions & 7 deletions fpsdk_common/src/parser/rtcm3.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -305,6 +305,36 @@ void Rtcm3SetSigned(uint8_t* data, const std::size_t offs, const std::size_t siz

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

const char* Rtcm3MsmGnssStr(const Rtcm3MsmGnss gnss)
{
switch (gnss) { // clang-format off
case Rtcm3MsmGnss::GPS: return "GPS";
case Rtcm3MsmGnss::GLO: return "GLO";
case Rtcm3MsmGnss::GAL: return "GAL";
case Rtcm3MsmGnss::SBAS: return "SBAS";
case Rtcm3MsmGnss::QZSS: return "QZSS";
case Rtcm3MsmGnss::BDS: return "BDS";
case Rtcm3MsmGnss::NAVIC: return "NAVIC";
} // clang-format on
return "?";
}

const char* Rtcm3MsmTypeStr(const Rtcm3MsmType gnss)
{
switch (gnss) { // clang-format off
case Rtcm3MsmType::MSM1: return "MSM1";
case Rtcm3MsmType::MSM2: return "MSM2";
case Rtcm3MsmType::MSM3: return "MSM3";
case Rtcm3MsmType::MSM4: return "MSM4";
case Rtcm3MsmType::MSM5: return "MSM5";
case Rtcm3MsmType::MSM6: return "MSM6";
case Rtcm3MsmType::MSM7: return "MSM7";
} // clang-format on
return "?";
}

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

bool Rtcm3GetArp(const uint8_t* msg, Rtcm3Arp& arp)
{
bool ok = false;
Expand Down Expand Up @@ -446,17 +476,17 @@ bool Rtcm3GetMsmHeader(const uint8_t* msg, Rtcm3MsmHeader& header)
// clang-format off
header.multi_msg_bit_ = Rtcm3GetUnsigned(data, 54, 1); // DF393
header.iods_ = Rtcm3GetUnsigned(data, 55, 3); // DF409
// bit(7) reserved // DF001
// bit(7) reserved // DF001
header.clk_steering_ = Rtcm3GetUnsigned(data, 65, 2); // DF411
header.ext_clock_ = Rtcm3GetUnsigned(data, 67, 2); // DF412
header.smooth_ = Rtcm3GetUnsigned(data, 69, 1); // DF417
header.smooth_int_ = Rtcm3GetUnsigned(data, 70, 3); // DF418
header.sat_mask_ = Rtcm3GetUnsigned(data, 73, 64); // DF394
header.sig_mask_ = Rtcm3GetUnsigned(data, 137, 32); // DF395
header.sig_mask_ = Rtcm3GetUnsigned(data, 137, 32); // DF395
header.num_sat_ = Rtcm3CountBits(header.sat_mask_);
header.num_sig_ = Rtcm3CountBits(header.sig_mask_);
header.num_cell_ = header.num_sat_ * header.num_sig_;
header.cell_mask_ = Rtcm3GetUnsigned(data, 169, header.num_cell_); // DF396
header.cell_mask_ = Rtcm3GetUnsigned(data, 169, header.num_sat_ * header.num_sig_); // DF396
header.num_cell_ = Rtcm3CountBits(header.cell_mask_);
// clang-format on

return true;
Expand Down Expand Up @@ -493,12 +523,27 @@ bool Rtcm3GetMessageInfo(char* info, const std::size_t size, const uint8_t* msg,

Rtcm3MsmHeader msm;
if ((len < size) && Rtcm3GetMsmHeader(msg, msm)) {
len = snprintf(info, size, "(#%d) %010.3f (%d * %d, %s)", msm.ref_sta_id_, msm.any_tow_, msm.num_sat_,
msm.num_sig_, msm.multi_msg_bit_ ? "more" : "last");
// Calculate expected message size.
// clang-format off
std::size_t exp_size = (RTCM3_FRAME_SIZE * 8) + /* header: */ 169 + msm.num_cell_; // + sat data + sig data
switch (msm.msm_) {
case Rtcm3MsmType::MSM1: exp_size += (msm.num_sat_ * 10) + (msm.num_cell_ * 15); break;
case Rtcm3MsmType::MSM2: exp_size += (msm.num_sat_ * 10) + (msm.num_cell_ * 27); break;
case Rtcm3MsmType::MSM3: exp_size += (msm.num_sat_ * 10) + (msm.num_cell_ * 42); break;
case Rtcm3MsmType::MSM4: exp_size += (msm.num_sat_ * 18) + (msm.num_cell_ * 48); break;
case Rtcm3MsmType::MSM5: exp_size += (msm.num_sat_ * 36) + (msm.num_cell_ * 63); break;
case Rtcm3MsmType::MSM6: exp_size += (msm.num_sat_ * 18) + (msm.num_cell_ * 65); break;
case Rtcm3MsmType::MSM7: exp_size += (msm.num_sat_ * 36) + (msm.num_cell_ * 80); break; // clang-format on
}
exp_size = (exp_size + 7) / 8; // bytes

len = snprintf(info, size, "(#%d) %010.3f %s %s: %d sat, %d sig, %d cell, %s, exps %" PRIuMAX, msm.ref_sta_id_,
msm.any_tow_, Rtcm3MsmGnssStr(msm.gnss_), Rtcm3MsmTypeStr(msm.msm_), msm.num_sat_, msm.num_sig_,
msm.num_cell_, msm.multi_msg_bit_ ? "more" : "last", exp_size);
}

const char* type_desc = Rtcm3GetTypeDesc(Rtcm3Type(msg));
if ((len < size) && (type_desc != NULL)) {
if ((len == 0) && (len < size) && (type_desc != NULL)) {
len += snprintf(&info[len], size - len, "%s%s", len > 0 ? " - " : "", type_desc);
}

Expand Down
2 changes: 1 addition & 1 deletion fpsdk_common/test/parser_rtcm3_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -134,7 +134,7 @@ TEST(ParserRtcm3Test, Rtcm3GetMsmHeader)
EXPECT_FALSE(msm.multi_msg_bit_);
EXPECT_EQ(msm.num_sat_, 9);
EXPECT_EQ(msm.num_sig_, 2);
EXPECT_EQ(msm.num_cell_, 18);
EXPECT_EQ(msm.num_cell_, 13);
}

/* ****************************************************************************************************************** */
Expand Down
Loading