add QGC parser, types and utils

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

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

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})

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

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 <cstdint>
+
+/* 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__

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)
  *
  * @{
  */

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
 ///@}
 

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<MsgInfo, " . ($#QGC_GROUPS + 1) . "> 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<MsgInfo, " . ($#QGC_MESSAGES + 1) . "> 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