From 0991e9368007aa50381026e5c7d2ea5eaf94af42 Mon Sep 17 00:00:00 2001 From: flipflip Date: Wed, 10 Dec 2025 07:56:05 +0100 Subject: [PATCH] add QGC parser, types and utils --- fpsdk_apps/fpltool/fpltool_utils.cpp | 6 +- fpsdk_apps/parsertool/parsertool.cpp | 1 + fpsdk_common/CMakeLists.txt | 1 + fpsdk_common/include/fpsdk_common/parser.hpp | 1 + .../include/fpsdk_common/parser/qgc.hpp | 141 ++++++++++++++++++ .../include/fpsdk_common/parser/sbf.hpp | 2 +- .../include/fpsdk_common/parser/types.hpp | 9 +- fpsdk_common/src/codegen.pl | 72 ++++++++- fpsdk_common/src/parser.cpp | 55 ++++++- fpsdk_common/src/parser/qgc.cpp | 126 ++++++++++++++++ fpsdk_common/src/parser/types.cpp | 10 ++ fpsdk_common/test/parser_qgc_test.cpp | 99 ++++++++++++ fpsdk_common/test/parser_sbf_test.cpp | 5 +- fpsdk_ros1/msg/ParserMsg.msg | 3 +- fpsdk_ros1/test/msgs_test.cpp | 2 +- 15 files changed, 517 insertions(+), 16 deletions(-) create mode 100644 fpsdk_common/include/fpsdk_common/parser/qgc.hpp create mode 100644 fpsdk_common/src/parser/qgc.cpp create mode 100644 fpsdk_common/test/parser_qgc_test.cpp diff --git a/fpsdk_apps/fpltool/fpltool_utils.cpp b/fpsdk_apps/fpltool/fpltool_utils.cpp index dadf830..96af156 100644 --- a/fpsdk_apps/fpltool/fpltool_utils.cpp +++ b/fpsdk_apps/fpltool/fpltool_utils.cpp @@ -171,9 +171,11 @@ void ParserMsgHelper::UpdateParserMsg(const common::fpl::StreamMsg& streammsg) case Protocol::RTCM3: rosmsg_.protocol = fpsdk_ros1::ParserMsg::PROTOCOL_RTCM3; break; case Protocol::UNI_B: rosmsg_.protocol = fpsdk_ros1::ParserMsg::PROTOCOL_UNI_B; break; case Protocol::NOV_B: rosmsg_.protocol = fpsdk_ros1::ParserMsg::PROTOCOL_NOV_B; break; - case Protocol::SBF: rosmsg_.protocol = fpsdk_ros1::ParserMsg::PROTOCOL_SBF; break; case Protocol::SPARTN: rosmsg_.protocol = fpsdk_ros1::ParserMsg::PROTOCOL_SPARTN; break; - case Protocol::OTHER: rosmsg_.protocol = fpsdk_ros1::ParserMsg::PROTOCOL_OTHER; break; + case Protocol::SBF: rosmsg_.protocol = fpsdk_ros1::ParserMsg::PROTOCOL_SBF; break; + case Protocol::QGC: rosmsg_.protocol = fpsdk_ros1::ParserMsg::PROTOCOL_QGC; break; + case Protocol::OTHER: + default: rosmsg_.protocol = fpsdk_ros1::ParserMsg::PROTOCOL_OTHER; break; } // clang-format on #elif defined(FPSDK_USE_ROS2) // @todo implement for ROS2 diff --git a/fpsdk_apps/parsertool/parsertool.cpp b/fpsdk_apps/parsertool/parsertool.cpp index 36036f0..f17b07c 100644 --- a/fpsdk_apps/parsertool/parsertool.cpp +++ b/fpsdk_apps/parsertool/parsertool.cpp @@ -336,6 +336,7 @@ class ParserTool std::printf(fmt, ProtocolStr(Protocol::NOV_B), s.n_novb_, (double)s.n_novb_ * p_n, s.s_novb_, (double)s.s_novb_ * p_s); std::printf(fmt, ProtocolStr(Protocol::UNI_B), s.n_unib_, (double)s.n_unib_ * p_n, s.s_unib_, (double)s.s_unib_ * p_s); std::printf(fmt, ProtocolStr(Protocol::SBF), s.n_sbf_, (double)s.n_sbf_ * p_n, s.s_sbf_, (double)s.s_sbf_ * p_s); + std::printf(fmt, ProtocolStr(Protocol::QGC), s.n_qgc_, (double)s.n_qgc_ * p_n, s.s_qgc_, (double)s.s_qgc_ * p_s); std::printf(fmt, ProtocolStr(Protocol::SPARTN), s.n_spartn_, (double)s.n_spartn_ * p_n, s.s_spartn_, (double)s.s_spartn_ * p_s); std::printf(fmt, ProtocolStr(Protocol::OTHER), s.n_other_, (double)s.n_other_ * p_n, s.s_other_, (double)s.s_other_ * p_s); // clang-format on diff --git a/fpsdk_common/CMakeLists.txt b/fpsdk_common/CMakeLists.txt index 8794c2b..699fd31 100644 --- a/fpsdk_common/CMakeLists.txt +++ b/fpsdk_common/CMakeLists.txt @@ -235,6 +235,7 @@ add_gtest(TARGET parser_fpa_test SOURCES test/parser_fpa_test.cpp LINK add_gtest(TARGET parser_fpb_test SOURCES test/parser_fpb_test.cpp LINK_LIBS ${PROJECT_NAME}) add_gtest(TARGET parser_nmea_test SOURCES test/parser_nmea_test.cpp LINK_LIBS ${PROJECT_NAME}) add_gtest(TARGET parser_novb_test SOURCES test/parser_novb_test.cpp LINK_LIBS ${PROJECT_NAME}) +add_gtest(TARGET parser_qgc_test SOURCES test/parser_qgc_test.cpp LINK_LIBS ${PROJECT_NAME}) add_gtest(TARGET parser_rtcm3_test SOURCES test/parser_rtcm3_test.cpp LINK_LIBS ${PROJECT_NAME}) add_gtest(TARGET parser_sbf_test SOURCES test/parser_sbf_test.cpp LINK_LIBS ${PROJECT_NAME}) add_gtest(TARGET parser_spartn_test SOURCES test/parser_spartn_test.cpp LINK_LIBS ${PROJECT_NAME}) diff --git a/fpsdk_common/include/fpsdk_common/parser.hpp b/fpsdk_common/include/fpsdk_common/parser.hpp index 2bb54f0..d9b8183 100644 --- a/fpsdk_common/include/fpsdk_common/parser.hpp +++ b/fpsdk_common/include/fpsdk_common/parser.hpp @@ -36,6 +36,7 @@ * - @subpage FPSDK_COMMON_PARSER_FPB * - @subpage FPSDK_COMMON_PARSER_NMEA * - @subpage FPSDK_COMMON_PARSER_NOVB + * - @subpage FPSDK_COMMON_PARSER_QGC * - @subpage FPSDK_COMMON_PARSER_RTCM3 * - @subpage FPSDK_COMMON_PARSER_SBF * - @subpage FPSDK_COMMON_PARSER_SPARTN diff --git a/fpsdk_common/include/fpsdk_common/parser/qgc.hpp b/fpsdk_common/include/fpsdk_common/parser/qgc.hpp new file mode 100644 index 0000000..7ab447f --- /dev/null +++ b/fpsdk_common/include/fpsdk_common/parser/qgc.hpp @@ -0,0 +1,141 @@ +/** + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ + * + * Based on work by flipflip (https://github.com/phkehl) + * The information on message structures, IDs, descriptions etc. in this file are from publicly available data, such as: + * - LG290P&LGx80P Series, GNSS Protocol Specification, copyright Quectel Wireless Solutions Co + * \endverbatim + * + * @file + * @brief Fixposition SDK: Parser QGC routines and types + * + * @page FPSDK_COMMON_PARSER_QGC Parser QGC routines and types + * + * **API**: fpsdk_common/parser/qgc.hpp and fpsdk::common::parser::qgc + * + */ +#ifndef __FPSDK_COMMON_PARSER_QGC_HPP__ +#define __FPSDK_COMMON_PARSER_QGC_HPP__ + +/* LIBC/STL */ +#include + +/* EXTERNAL */ + +/* PACKAGE */ + +namespace fpsdk { +namespace common { +namespace parser { +/** + * @brief Parser QGC routines and types + */ +namespace qgc { +/* ****************************************************************************************************************** */ + +static constexpr uint8_t QGC_SYNC_1 = 0x51; //!< Sync char 1 ('Q') +static constexpr uint8_t QGC_SYNC_2 = 0x47; //!< Sync char 2 ('G') +static constexpr std::size_t QGC_HEAD_SIZE = 6; //!< Size of the QGC header +static constexpr std::size_t QGC_FRAME_SIZE = 8; //!< Size (in bytes) of QGC frame + +/** + * @brief Get group ID from message + * + * @param[in] msg Pointer to the start of the message + * + * @note No check on the data provided is done. This is meant for use as a helper in other functions. Checks on the + * \c msg and its data should be carried out there. + * + * @returns the QGC group ID + */ +constexpr uint8_t QgcGrpId(const uint8_t* msg) +{ + return (((uint8_t*)(msg))[2]); +} + +/** + * @brief Get message ID from message + * + * @param[in] msg Pointer to the start of the message + * + * @note No check on the data provided is done. This is meant for use as a helper in other functions. Checks on the + * \c msg and its data should be carried out there. + * + * @returns the QGC message ID + */ +constexpr uint8_t QgcMsgId(const uint8_t* msg) +{ + return (((uint8_t*)(msg))[3]); +} + +/** + * @brief Get QGC message name + * + * Generates a name (string) in the form "QGC-GRPID-MSGID", where GRPID and MSGID are suitable stringifications of the + * class ID and message ID if known (for example, "QGC-RAW-HASE6", respectively %02X formatted IDs if unknown (for + * example, "QGC-0A-FF"). + * + * @param[out] name String to write the name to + * @param[in] size Size of \c name (incl. nul termination) + * @param[in] msg Pointer to the QGC message + * @param[in] msg_size Size of the \c msg + * + * @note No check on the data provided is done. The caller must ensure that the data is a valid QGC message. + * + * @returns true if message name was generated, false if \c name buffer was too small + */ +bool QgcGetMessageName(char* name, const std::size_t size, const uint8_t* msg, const std::size_t msg_size); + +/** + * @brief Get QGC message info + * + * This stringifies the content of some QGC messages, for debugging. + * + * @param[out] info String to write the info to + * @param[in] size Size of \c name (incl. nul termination) + * @param[in] msg Pointer to the QGC message + * @param[in] msg_size Size of the \c msg + * + * @note No check on the data provided is done. The caller must ensure that the data is a valid QGC message. + * + * @returns true if message info was generated (even if info is empty), false if \c name buffer was too small + */ +bool QgcGetMessageInfo(char* info, const std::size_t size, const uint8_t* msg, const std::size_t msg_size); + +// --------------------------------------------------------------------------------------------------------------------- + +/** + * @name QGC groups and messages (names and IDs) + * + * @{ + */ +// clang-format off +// @fp_codegen_begin{FPSDK_COMMON_PARSER_QGC_GROUPS} +static constexpr uint8_t QGC_RAW_GRPID = 0x0a; //!< QGC-RAW group ID +static constexpr const char* QGC_RAW_STRID = "QGC-RAW"; //!< QGC-RAW group name +// @fp_codegen_end{FPSDK_COMMON_PARSER_QGC_GROUPS} +// clang-format on + +// clang-format off +// @fp_codegen_begin{FPSDK_COMMON_PARSER_QGC_MESSAGES} +static constexpr uint8_t QGC_RAW_PPPB2B_MSGID = 0xb2; //!< QGC-RAW-PPPB2B message ID +static constexpr const char* QGC_RAW_PPPB2B_STRID = "QGC-RAW-PPPB2B"; //!< QGC-RAW-PPPB2B message name +static constexpr uint8_t QGC_RAW_QZSSL6_MSGID = 0xb6; //!< QGC-RAW-QZSSL6 message ID +static constexpr const char* QGC_RAW_QZSSL6_STRID = "QGC-RAW-QZSSL6"; //!< QGC-RAW-QZSSL6 message name +static constexpr uint8_t QGC_RAW_HASE6_MSGID = 0xe6; //!< QGC-RAW-HASE6 message ID +static constexpr const char* QGC_RAW_HASE6_STRID = "QGC-RAW-HASE6"; //!< QGC-RAW-HASE6 message name +// @fp_codegen_end{FPSDK_COMMON_PARSER_QGC_MESSAGES} +// clang-format on +///@} + +/* ****************************************************************************************************************** */ +} // namespace qgc +} // namespace parser +} // namespace common +} // namespace fpsdk +#endif // __FPSDK_COMMON_PARSER_QGC_HPP__ diff --git a/fpsdk_common/include/fpsdk_common/parser/sbf.hpp b/fpsdk_common/include/fpsdk_common/parser/sbf.hpp index ac2058a..bf356c4 100644 --- a/fpsdk_common/include/fpsdk_common/parser/sbf.hpp +++ b/fpsdk_common/include/fpsdk_common/parser/sbf.hpp @@ -121,7 +121,7 @@ bool SbfGetMessageInfo(char* info, const std::size_t size, const uint8_t* msg, c // --------------------------------------------------------------------------------------------------------------------- /** - * @name UNI_B messages (names and IDs) + * @name SBF messages (names and IDs) * * @{ */ diff --git a/fpsdk_common/include/fpsdk_common/parser/types.hpp b/fpsdk_common/include/fpsdk_common/parser/types.hpp index 5eab6f1..cf20859 100644 --- a/fpsdk_common/include/fpsdk_common/parser/types.hpp +++ b/fpsdk_common/include/fpsdk_common/parser/types.hpp @@ -49,6 +49,7 @@ enum class Protocol : int UNI_B, //!< UNI_B (Unicore proprietary binary) (#PROTOCOL_NAME_UNI_B) NOV_B, //!< NOV_B (NovAtel proprietary binary, long or short header) (#PROTOCOL_NAME_NOV_B) SBF, //!< SBF (Septentrio binary format) (#PROTOCOL_NAME_SBF) + QGC, //!< QGC (Quectel proprietary binary) (#PROTOCOL_NAME_QGC) SPARTN, //!< SPARTN (#PROTOCOL_NAME_SPARTN) OTHER, //!< Other "message" (unknown or corrupt message, spurious data, line noise, ...) (#PROTOCOL_NAME_OTHER) }; @@ -67,6 +68,7 @@ static constexpr const char* PROTOCOL_NAME_RTCM3 = "RTCM3"; //!< Name (label) static constexpr const char* PROTOCOL_NAME_UNI_B = "UNI_B"; //!< Name (label) for Protocol::UNI_B static constexpr const char* PROTOCOL_NAME_NOV_B = "NOV_B"; //!< Name (label) for Protocol::NOV_B static constexpr const char* PROTOCOL_NAME_SBF = "SBF"; //!< Name (label) for Protocol::SBF +static constexpr const char* PROTOCOL_NAME_QGC = "QGC"; //!< Name (label) for Protocol::QGC static constexpr const char* PROTOCOL_NAME_SPARTN = "SPARTN"; //!< Name (label) for Protocol::SPARTN static constexpr const char* PROTOCOL_NAME_OTHER = "OTHER"; //!< Name (label) for Protocol::OTHER // clang-format on @@ -150,6 +152,8 @@ struct ParserStats uint64_t s_novb_ = 0; //!< Total size of Protocol::NOV_B messages uint64_t n_sbf_ = 0; //!< Number of Protocol::SBF messages uint64_t s_sbf_ = 0; //!< Total size of Protocol::SBF messages + uint64_t n_qgc_ = 0; //!< Number of Protocol::QGC messages + uint64_t s_qgc_ = 0; //!< Total size of Protocol::QGC messages uint64_t n_spartn_ = 0; //!< Number of Protocol::SPARTN messages uint64_t s_spartn_ = 0; //!< Total size of Protocol::SPARTN messages uint64_t n_other_ = 0; //!< Number of Protocol::OTHER messages @@ -180,11 +184,12 @@ static constexpr std::size_t MAX_SPARTN_SIZE = 1110; //!< Maximum SPARTN mes static constexpr std::size_t MAX_NOV_B_SIZE = 4096; //!< Maximum NOV_B message size static constexpr std::size_t MAX_UNI_B_SIZE = 4096; //!< Maximum UNI_B message size static constexpr std::size_t MAX_SBF_SIZE = 4608; //!< Maximum SBF message size +static constexpr std::size_t MAX_QGC_SIZE = 4608; //!< Maximum QGC message size static constexpr std::size_t MAX_OTHER_SIZE = 256; //!< Maximum OTHER message size static constexpr std::size_t MAX_ANY_SIZE = std::max({ MAX_NMEA_SIZE, MAX_FP_A_SIZE, MAX_FP_B_SIZE, MAX_UBX_SIZE, - MAX_RTCM3_SIZE, MAX_SPARTN_SIZE, MAX_NOV_B_SIZE, MAX_UNI_B_SIZE, MAX_SBF_SIZE, MAX_OTHER_SIZE }); //!< The largest of the above + MAX_RTCM3_SIZE, MAX_SPARTN_SIZE, MAX_NOV_B_SIZE, MAX_UNI_B_SIZE, MAX_SBF_SIZE, MAX_QGC_SIZE, MAX_OTHER_SIZE }); //!< The largest of the above static constexpr std::size_t MIN_ANY_SIZE = std::min({ MAX_NMEA_SIZE, MAX_FP_A_SIZE, MAX_FP_B_SIZE, MAX_UBX_SIZE, - MAX_RTCM3_SIZE, MAX_SPARTN_SIZE, MAX_NOV_B_SIZE, MAX_UNI_B_SIZE, MAX_SBF_SIZE, MAX_OTHER_SIZE }); //!< The smallest of the above + MAX_RTCM3_SIZE, MAX_SPARTN_SIZE, MAX_NOV_B_SIZE, MAX_UNI_B_SIZE, MAX_SBF_SIZE, MAX_QGC_SIZE, MAX_OTHER_SIZE }); //!< The smallest of the above // clang-format on ///@} diff --git a/fpsdk_common/src/codegen.pl b/fpsdk_common/src/codegen.pl index f681996..1181660 100755 --- a/fpsdk_common/src/codegen.pl +++ b/fpsdk_common/src/codegen.pl @@ -154,8 +154,6 @@ { name => 'RTCM3', clsid => 0xf5 }, ); -######################################################################################################################## - my @UBX_MESSAGES = ( { class => 'ACK', name => 'ACK', msgid => 0x01 }, @@ -634,6 +632,20 @@ ######################################################################################################################## +my @QGC_GROUPS = +( + { name => 'RAW', grpid => 0x0a }, +); + +my @QGC_MESSAGES = +( + { class => 'RAW', name => 'PPPB2B', msgid => 0xb2 }, + { class => 'RAW', name => 'QZSSL6', msgid => 0xb6 }, + { class => 'RAW', name => 'HASE6', msgid => 0xe6 }, +); + +######################################################################################################################## + my $FPSDK_COMMON_DIR = path("$FindBin::Bin/..")->canonpath(); my @CODEGEN_FILES = (); my %CODEGEN_SECTIONS = (); @@ -918,6 +930,62 @@ "}};\n"); }; +######################################################################################################################## +# Generate code for QGC +do +{ + push(@CODEGEN_FILES, + path("$FPSDK_COMMON_DIR/include/fpsdk_common/parser/qgc.hpp"), + path("$FPSDK_COMMON_DIR/src/parser/qgc.cpp")); + $CODEGEN_SECTIONS{FPSDK_COMMON_PARSER_QGC_GROUPS} = []; + $CODEGEN_SECTIONS{FPSDK_COMMON_PARSER_QGC_MSGINFO} = [ + "static constexpr std::array GRP_INFO =\n", + "{{\n", + ]; + foreach my $entry (@QGC_GROUPS) + { + my $name = "QGC-$entry->{name}"; + my $grpid = "QGC_$entry->{name}_GRPID"; + my $strid = "QGC_$entry->{name}_STRID"; + # print(Dumper($entry);) + push(@{$CODEGEN_SECTIONS{FPSDK_COMMON_PARSER_QGC_GROUPS}}, + sprintf("static constexpr uint8_t %-30s = 0x%02x; //!< $name group ID\n", $grpid, $entry->{grpid}), + sprintf("static constexpr const char* %-30s = %-25s //!< $name group name\n", $strid, "\"${name}\";"), + ); + push(@{$CODEGEN_SECTIONS{FPSDK_COMMON_PARSER_QGC_MSGINFO}}, + sprintf(" { %-30s 0x00, %-30s },\n", "$grpid,", $strid), + ); + } + push(@{$CODEGEN_SECTIONS{FPSDK_COMMON_PARSER_QGC_MSGINFO}}, + "}};\n", + ); + + push(@{$CODEGEN_SECTIONS{FPSDK_COMMON_PARSER_QGC_MSGINFO}}, + "static constexpr std::array MSG_INFO =\n", + "{{\n", + ); + foreach my $entry (@QGC_MESSAGES) + { + my $name = "QGC-$entry->{class}-$entry->{name}"; + my $grpid = "QGC_$entry->{class}_GRPID"; + my $msgid = "QGC_$entry->{class}_$entry->{name}_MSGID"; + my $strid = "QGC_$entry->{class}_$entry->{name}_STRID"; + push(@{$CODEGEN_SECTIONS{FPSDK_COMMON_PARSER_QGC_MESSAGES}}, + ($entry->{msgid} =~ m{^QGC} ? + sprintf("static constexpr uint8_t %-30s = %-24s //!< $name message ID\n", $msgid, "$entry->{msgid};") : + sprintf("static constexpr uint8_t %-30s = 0x%02x; //!< $name message ID\n", $msgid, $entry->{msgid}) + ), + sprintf("static constexpr const char* %-30s = %-25s //!< $name message name\n", "QGC_$entry->{class}_$entry->{name}_STRID", "\"${name}\";"), + ); + push(@{$CODEGEN_SECTIONS{FPSDK_COMMON_PARSER_QGC_MSGINFO}}, + sprintf(" { %-30s %-30s %-30s },\n", "$grpid,", "$msgid,", $strid), + ); + } + push(@{$CODEGEN_SECTIONS{FPSDK_COMMON_PARSER_QGC_MSGINFO}}, + "}};\n", + ); +}; + ######################################################################################################################## # Update files as necessary diff --git a/fpsdk_common/src/parser.cpp b/fpsdk_common/src/parser.cpp index f10eb53..62b91e2 100644 --- a/fpsdk_common/src/parser.cpp +++ b/fpsdk_common/src/parser.cpp @@ -29,6 +29,7 @@ #include "fpsdk_common/parser/fpb.hpp" #include "fpsdk_common/parser/nmea.hpp" #include "fpsdk_common/parser/novb.hpp" +#include "fpsdk_common/parser/qgc.hpp" #include "fpsdk_common/parser/rtcm3.hpp" #include "fpsdk_common/parser/sbf.hpp" #include "fpsdk_common/parser/spartn.hpp" @@ -45,6 +46,7 @@ using namespace fpsdk::common::parser::fpa; using namespace fpsdk::common::parser::fpb; using namespace fpsdk::common::parser::nmea; using namespace fpsdk::common::parser::novb; +using namespace fpsdk::common::parser::qgc; using namespace fpsdk::common::parser::rtcm3; using namespace fpsdk::common::parser::sbf; using namespace fpsdk::common::parser::spartn; @@ -114,8 +116,6 @@ static int IsUbxMessage(const uint8_t* buf, const std::size_t size); static int IsNmeaMessage(const uint8_t* buf, const std::size_t size); //! Check for presence of a RTCM3 message in the buffer (see IsMessageFunc) static int IsRtcm3Message(const uint8_t* buf, const std::size_t size); -//! Check for presence of a SBF message in the buffer (see IsMessageFunc) -static int IsSbfMessage(const uint8_t* buf, const std::size_t size); //! Check for presence of a SPARTN message in the buffer (see IsMessageFunc) static int IsSpartnMessage(const uint8_t* buf, const std::size_t size); //! Check for presence of a NOV_B message in the buffer (see IsMessageFunc) @@ -124,6 +124,10 @@ static int IsNovbMessage(const uint8_t* buf, const std::size_t size); static int IsFpbMessage(const uint8_t* buf, const std::size_t size); //! Check for presence of a UNI_B message in the buffer (see IsMessageFunc) static int IsUnibMessage(const uint8_t* buf, const std::size_t size); +//! Check for presence of a SBF message in the buffer (see IsMessageFunc) +static int IsSbfMessage(const uint8_t* buf, const std::size_t size); +//! Check for presence of a QGC message in the buffer (see IsMessageFunc) +static int IsQgcMessage(const uint8_t* buf, const std::size_t size); //! Helper for iterating through the different message frame detectors struct ParserFunc @@ -134,7 +138,7 @@ struct ParserFunc }; //! List of different message frame detectors -static const std::array PARSER_FUNCS = { { +static const std::array PARSER_FUNCS = { { // clang-format off { IsFpbMessage, Protocol::FP_B, PROTOCOL_NAME_FP_B }, { IsUbxMessage, Protocol::UBX, PROTOCOL_NAME_UBX }, @@ -143,6 +147,7 @@ static const std::array PARSER_FUNCS = { { { IsUnibMessage, Protocol::UNI_B, PROTOCOL_NAME_UNI_B }, { IsNovbMessage, Protocol::NOV_B, PROTOCOL_NAME_NOV_B }, { IsSbfMessage, Protocol::SBF, PROTOCOL_NAME_SBF }, + { IsQgcMessage, Protocol::QGC, PROTOCOL_NAME_QGC }, { IsSpartnMessage, Protocol::SPARTN, PROTOCOL_NAME_SPARTN }, }}; // clang-format on @@ -300,6 +305,9 @@ void Parser::EmitMessage(ParserMsg& msg, const std::size_t size, const Protocol case Protocol::SBF: msg.name_ = (SbfGetMessageName(sname, sizeof(sname), mdata, msize) ? sname : "SBF-?"); // GCOVR_EXCL_LINE break; + case Protocol::QGC: + msg.name_ = (QgcGetMessageName(sname, sizeof(sname), mdata, msize) ? sname : "Qgc-?"); // GCOVR_EXCL_LINE + break; case Protocol::SPARTN: msg.name_ = (SpartnGetMessageName(sname, sizeof(sname), mdata, msize) ? sname : "SPARTN-?-?"); // GCOVR_EXCL_LINE break; @@ -779,6 +787,47 @@ static int IsSbfMessage(const uint8_t* buf, const std::size_t size) return NADA; } +// --------------------------------------------------------------------------------------------------------------------- + +// Like IsUbxMessage(), but with different sync chars +static int IsQgcMessage(const uint8_t* buf, const std::size_t size) +{ + // We need the two sync chars + if (buf[0] != QGC_SYNC_1) { + return NADA; + } + if (size < 2) { + return WAIT; + } + if (buf[1] != QGC_SYNC_2) { + return NADA; + } + + // Wait for full header + if (size < QGC_HEAD_SIZE) { + return WAIT; + } + + // Limit payload size + const std::size_t payload_size = ((uint16_t)buf[4] | ((uint16_t)buf[5] << 8)); + if (payload_size > (MAX_QGC_SIZE - QGC_FRAME_SIZE)) { + return NADA; + } + + // Wait for entire message + if (size < (payload_size + QGC_FRAME_SIZE)) { + return WAIT; + } + + // Verify using checksum + const uint16_t ck = *((uint16_t*)&buf[payload_size + QGC_HEAD_SIZE]); + if (ck == ChecksumUbx(&buf[2], payload_size + (QGC_HEAD_SIZE - 2))) { + return payload_size + QGC_FRAME_SIZE; + } + + return NADA; +} + /* ****************************************************************************************************************** */ } // namespace parser } // namespace common diff --git a/fpsdk_common/src/parser/qgc.cpp b/fpsdk_common/src/parser/qgc.cpp new file mode 100644 index 0000000..35937af --- /dev/null +++ b/fpsdk_common/src/parser/qgc.cpp @@ -0,0 +1,126 @@ +/** + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ + * + * Based on work by flipflip (https://github.com/phkehl) + * \endverbatim + * + * @file + * @brief Fixposition SDK: Parser QGC routines and types + */ + +/* LIBC/STL */ +#include +#include +#include + +/* EXTERNAL */ + +/* PACKAGE */ +#include "fpsdk_common/parser/qgc.hpp" + +namespace fpsdk { +namespace common { +namespace parser { +namespace qgc { +/* ****************************************************************************************************************** */ + +// Lookup table entry +struct MsgInfo +{ + uint8_t grp_id_ = 0; + uint8_t msg_id_ = 0; + const char* name_ = nullptr; +}; + +// clang-format off +// @fp_codegen_begin{FPSDK_COMMON_PARSER_QGC_MSGINFO} +static constexpr std::array GRP_INFO = +{{ + { QGC_RAW_GRPID, 0x00, QGC_RAW_STRID }, +}}; +static constexpr std::array MSG_INFO = +{{ + { QGC_RAW_GRPID, QGC_RAW_PPPB2B_MSGID, QGC_RAW_PPPB2B_STRID }, + { QGC_RAW_GRPID, QGC_RAW_QZSSL6_MSGID, QGC_RAW_QZSSL6_STRID }, + { QGC_RAW_GRPID, QGC_RAW_HASE6_MSGID, QGC_RAW_HASE6_STRID }, +}}; +// @fp_codegen_end{FPSDK_COMMON_PARSER_QGC_MSGINFO} +// clang-format on + +// --------------------------------------------------------------------------------------------------------------------- + +bool QgcGetMessageName(char* name, const std::size_t size, const uint8_t* msg, const std::size_t msg_size) +{ + // Check arguments + if ((name == NULL) || (size < 1)) { + return false; + } + name[0] = '\0'; + + if ((msg == NULL) || (msg_size < QGC_FRAME_SIZE)) { + return false; + } + + const uint8_t grp_id = QgcGrpId(msg); + const uint8_t msg_id = QgcMsgId(msg); + + std::size_t res = 0; + + // First try the message name lookup table + for (auto& info : MSG_INFO) { + if ((info.grp_id_ == grp_id) && (info.msg_id_ == msg_id)) { + res = std::snprintf(name, size, "%s", info.name_); + break; + } + } + + // If that failed, try the class name lookup table + if (res == 0) { + for (auto& info : GRP_INFO) { + if (info.grp_id_ == grp_id) { + res = std::snprintf(name, size, "%s-%02" PRIX8, info.name_, msg_id); + break; + } + } + } + + // If that failed, too, stringify both IDs + if (res == 0) { + res = std::snprintf(name, size, "QGC-%02" PRIX8 "-%02" PRIX8, grp_id, msg_id); + } + + // Did it fit into the string? + return res < size; +} + +// --------------------------------------------------------------------------------------------------------------------- + +bool QgcGetMessageInfo(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 < QGC_HEAD_SIZE)) { + return false; + } + + info[0] = '\0'; + + if ((msg == NULL) || (msg_size < QGC_HEAD_SIZE)) { + return false; + } + + std::size_t len = 0; + + // TODO: implement some stringification + + return (len > 0) && (len < size); +} + +/* ****************************************************************************************************************** */ +} // namespace qgc +} // namespace parser +} // namespace common +} // namespace fpsdk diff --git a/fpsdk_common/src/parser/types.cpp b/fpsdk_common/src/parser/types.cpp index 62b6b02..239609f 100644 --- a/fpsdk_common/src/parser/types.cpp +++ b/fpsdk_common/src/parser/types.cpp @@ -24,6 +24,7 @@ #include "fpsdk_common/parser/fpb.hpp" #include "fpsdk_common/parser/nmea.hpp" #include "fpsdk_common/parser/novb.hpp" +#include "fpsdk_common/parser/qgc.hpp" #include "fpsdk_common/parser/rtcm3.hpp" #include "fpsdk_common/parser/sbf.hpp" #include "fpsdk_common/parser/spartn.hpp" @@ -47,6 +48,7 @@ const char* ProtocolStr(const Protocol proto) case Protocol::UNI_B: return PROTOCOL_NAME_UNI_B; case Protocol::NOV_B: return PROTOCOL_NAME_NOV_B; case Protocol::SBF: return PROTOCOL_NAME_SBF; + case Protocol::QGC: return PROTOCOL_NAME_QGC; case Protocol::SPARTN: return PROTOCOL_NAME_SPARTN; case Protocol::OTHER: return PROTOCOL_NAME_OTHER; } // clang-format on @@ -66,6 +68,7 @@ Protocol StrProtocol(const char* name) else if (std::strcmp(name, PROTOCOL_NAME_UNI_B) == 0) { return Protocol::UNI_B; } else if (std::strcmp(name, PROTOCOL_NAME_NOV_B) == 0) { return Protocol::NOV_B; } else if (std::strcmp(name, PROTOCOL_NAME_SBF) == 0) { return Protocol::SBF; } + else if (std::strcmp(name, PROTOCOL_NAME_QGC) == 0) { return Protocol::QGC; } else if (std::strcmp(name, PROTOCOL_NAME_SPARTN) == 0) { return Protocol::SPARTN; } } // clang-format on return Protocol::OTHER; @@ -109,6 +112,9 @@ void ParserMsg::MakeInfo() const case Protocol::SBF: info_ = (sbf::SbfGetMessageInfo(sinfo, sizeof(sinfo), mdata, msize) ? sinfo : ""); break; + case Protocol::QGC: + info_ = (qgc::QgcGetMessageInfo(sinfo, sizeof(sinfo), mdata, msize) ? sinfo : ""); + break; case Protocol::OTHER: { // Info is a hexdump of the first few bytes constexpr int num = 16; @@ -168,6 +174,10 @@ void ParserStats::Update(const ParserMsg& msg) n_sbf_++; s_sbf_ += size; break; + case Protocol::QGC: + n_qgc_++; + s_qgc_ += size; + break; case Protocol::SPARTN: n_spartn_++; s_spartn_ += size; diff --git a/fpsdk_common/test/parser_qgc_test.cpp b/fpsdk_common/test/parser_qgc_test.cpp new file mode 100644 index 0000000..d47e167 --- /dev/null +++ b/fpsdk_common/test/parser_qgc_test.cpp @@ -0,0 +1,99 @@ +/** + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ + * \endverbatim + * + * @file + * @brief Fixposition SDK: tests for fpsdk::common::parser::qgc + */ + +/* LIBC/STL */ +#include +#include + +/* EXTERNAL */ +#include + +/* PACKAGE */ +#include +#include + +namespace { +/* ****************************************************************************************************************** */ +using namespace fpsdk::common::parser::qgc; + +TEST(ParserQgcTest, Macros) +{ + { + const uint8_t msg[] = { 0x55, 0x55, 0x12, 0x34, 0xaa, 0xaa, 0xaa, 0xaa }; + ASSERT_EQ(QgcGrpId(msg), 0x12); + ASSERT_EQ(QgcMsgId(msg), 0x34); + } +} + +// --------------------------------------------------------------------------------------------------------------------- + +TEST(ParserSbfTest, QgcGetMessageName) +{ + // Known message + { + const uint8_t msg[] = { QGC_SYNC_1, QGC_SYNC_2, QGC_RAW_GRPID, QGC_RAW_HASE6_MSGID, 0x55, 0x55, 0xaa, 0xaa }; + char str[100]; + EXPECT_TRUE(QgcGetMessageName(str, sizeof(str), msg, sizeof(msg))); + EXPECT_EQ(std::string(str), std::string("QGC-RAW-HASE6")); + } + // Unknown message + { + const uint8_t msg[] = { QGC_SYNC_1, QGC_SYNC_2, QGC_RAW_GRPID, 0x9f, 0x55, 0x55, 0xaa, 0xaa }; + char str[100]; + EXPECT_TRUE(QgcGetMessageName(str, sizeof(str), msg, sizeof(msg))); + EXPECT_EQ(std::string(str), std::string("QGC-RAW-9F")); + } + // Unknown group and message + { + const uint8_t msg[] = { QGC_SYNC_1, QGC_SYNC_2, 0x8a, 0x7b, 0x55, 0x55, 0xaa, 0xaa }; + char str[100]; + EXPECT_TRUE(QgcGetMessageName(str, sizeof(str), msg, sizeof(msg))); + EXPECT_EQ(std::string(str), std::string("QGC-8A-7B")); + } + + // Bad arguments + { + const uint8_t msg[] = { QGC_SYNC_1, QGC_SYNC_2, QGC_RAW_GRPID, QGC_RAW_HASE6_MSGID, 0x55, 0x55, 0xaa, 0xaa }; + char str[100]; + EXPECT_FALSE(QgcGetMessageName(str, 0, msg, sizeof(msg))); + EXPECT_FALSE(QgcGetMessageName(NULL, 10, msg, sizeof(msg))); + EXPECT_FALSE(QgcGetMessageName(NULL, 0, msg, sizeof(msg))); + EXPECT_FALSE(QgcGetMessageName(str, sizeof(str), msg, 0)); + EXPECT_FALSE(QgcGetMessageName(str, sizeof(str), msg, 5)); + EXPECT_FALSE(QgcGetMessageName(str, sizeof(str), NULL, sizeof(msg))); + } + + // Too small string is cut + { + const uint8_t msg[] = { QGC_SYNC_1, QGC_SYNC_2, QGC_RAW_GRPID, QGC_RAW_HASE6_MSGID, 0x55, 0x55, 0xaa, 0xaa }; + char str[10]; + EXPECT_FALSE(QgcGetMessageName(str, sizeof(str), msg, sizeof(msg))); + EXPECT_EQ(std::string(str), std::string("QGC-RAW-H")); + } +} + +/* ****************************************************************************************************************** */ +} // namespace + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + auto level = fpsdk::common::logging::LoggingLevel::WARNING; + for (int ix = 0; ix < argc; ix++) { + if ((argv[ix][0] == '-') && argv[ix][1] == 'v') { + level++; + } + } + fpsdk::common::logging::LoggingSetParams(level); + return RUN_ALL_TESTS(); +} diff --git a/fpsdk_common/test/parser_sbf_test.cpp b/fpsdk_common/test/parser_sbf_test.cpp index ddc9781..bef7b4e 100644 --- a/fpsdk_common/test/parser_sbf_test.cpp +++ b/fpsdk_common/test/parser_sbf_test.cpp @@ -28,7 +28,6 @@ using namespace fpsdk::common::parser::sbf; TEST(ParserSbfTest, Macros) { - ASSERT_TRUE(true); { const uint8_t msg[] = { 0x55, 0x55, 0x55, 0x55, 0x34, 0x12, 0xaa, 0xaa, 0xaa }; ASSERT_EQ(SbfBlockType(msg), 0x1234); @@ -52,10 +51,8 @@ TEST(ParserSbfTest, Macros) // --------------------------------------------------------------------------------------------------------------------- -TEST(ParserSbfTest, UnibGetMessageName) +TEST(ParserSbfTest, SbfGetMessageName) { - ASSERT_TRUE(true); - // NavCart 4272 // Known message { const uint8_t msg[] = { // clang-format off diff --git a/fpsdk_ros1/msg/ParserMsg.msg b/fpsdk_ros1/msg/ParserMsg.msg index 1923e86..9a5fd32 100644 --- a/fpsdk_ros1/msg/ParserMsg.msg +++ b/fpsdk_ros1/msg/ParserMsg.msg @@ -16,8 +16,9 @@ int8 PROTOCOL_RTCM3 = 5 int8 PROTOCOL_UNI_B = 6 int8 PROTOCOL_NOV_B = 7 int8 PROTOCOL_SPARTN = 8 -int8 PROTOCOL_SBF = 10 int8 PROTOCOL_OTHER = 9 +int8 PROTOCOL_SBF = 10 +int8 PROTOCOL_QGC = 11 uint8[] data # Message data string name # Message name uint64 seq # Message counter diff --git a/fpsdk_ros1/test/msgs_test.cpp b/fpsdk_ros1/test/msgs_test.cpp index 87de73a..97d408b 100644 --- a/fpsdk_ros1/test/msgs_test.cpp +++ b/fpsdk_ros1/test/msgs_test.cpp @@ -33,7 +33,7 @@ namespace { TEST(MsgsTest, MustNeverChange) { // clang-format off - EXPECT_EQ(std::string(ros::message_traits::md5sum()), "34588c4466b4597f15baa5ca265d0a37"); + EXPECT_EQ(std::string(ros::message_traits::md5sum()), "74030d9f5f1291a0532d94562740ed9d"); // clang-format on }