Quectel PQTMEPE support (#33)

* Added 'compile_commands.json' to gitignore
* Added EPE message support
* release: 3.4.11
* clean up + formatting

---------

Co-authored-by: Axel Sundelin <axel.sundelin@ercisson.com>

.gitignore

@@ -4,4 +4,5 @@ bin
 build
 output.txt
 generated_files
-.vscode
+.vscode
+compile_commands.json

CHANGELOG.md

@@ -1,7 +1,10 @@
 # Changelog
 
 ## [3.4.*]
+
+## [3.4.11] 2024-09-26
 - Added support for UBX-RXM-RAWX messages.
+- Added support for $PQTMEPE messages.
 
 ## [3.4.10] 2024-08-07
 - Added new control command `/IDENTITY` to provide the client with the IMSI, MSISDN, or IP address. See `CONTROL.md` for more information. 

CMakeLists.txt

@@ -37,8 +37,8 @@ find_package(OpenSSL REQUIRED)
 endif (USE_OPENSSL)
 
 add_definitions(-D_POSIX_C_SOURCE=200809L)
-add_definitions(-DCLIENT_VERSION="3.4.10")
-add_definitions(-DCLIENT_VERSION_INT=0x030410)
+add_definitions(-DCLIENT_VERSION="3.4.11")
+add_definitions(-DCLIENT_VERSION_INT=0x030411)
 
 if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU")
    add_definitions(-DCOMPILER_CANNOT_DEDUCE_UNREACHABLE=1)

examples/lpp/location_information.cpp

@@ -108,28 +108,32 @@ PLI_Result provide_location_information_callback_nmea(LocationInformation& locat
     auto gga = receiver->gga();
     auto vtg = receiver->vtg();
     auto gst = receiver->gst();
-    if (!gga || !vtg || !gst) {
+    auto epe = receiver->epe();
+    if (!gga || !vtg || !(gst || epe)) {
         return PLI_Result::NOT_AVAILABLE;
     }
 
-    auto semi_major = gst->semi_major();
-    auto semi_minor = gst->semi_minor();
+    auto semi_major  = gst ? gst->semi_major() : epe->semi_major();
+    auto semi_minor  = gst ? gst->semi_minor() : epe->semi_minor();
+    auto orientation = gst ? gst->orientation() : epe->orientation();
+    auto vertical_position_error =
+        gst ? gst->vertical_position_error() : epe->vertical_position_error();
 
     if (gConvertConfidence95To39) {
         semi_major = semi_major / 2.4477;
         semi_minor = semi_minor / 2.4477;
     }
 
     auto horizontal_accuracy =
-        HorizontalAccuracy::from_ellipse(semi_major, semi_minor, gst->orientation());
+        HorizontalAccuracy::from_ellipse(semi_major, semi_minor, orientation);
     if (gOverrideHorizontalConfidence >= 0.0) {
         horizontal_accuracy.confidence = gOverrideHorizontalConfidence;
     }
 
     location.time     = gga->time_of_day();
     location.location = LocationShape::ha_ellipsoid_altitude_with_uncertainty(
         gga->latitude(), gga->longitude(), gga->altitude(), horizontal_accuracy,
-        VerticalAccuracy::from_1sigma(gst->vertical_position_error()));
+        VerticalAccuracy::from_1sigma(vertical_position_error));
     location.velocity =
         VelocityShape::horizontal(vtg->speed_over_ground(), vtg->true_course_over_ground());
 

receiver/nmea/CMakeLists.txt

@@ -7,6 +7,7 @@ add_library(receiver_nmea STATIC
     "gga.cpp"
     "vtg.cpp"
     "gst.cpp"
+    "epe.cpp"
 )
 add_library(receiver::nmea ALIAS receiver_nmea)
 

receiver/nmea/epe.cpp

@@ -0,0 +1,71 @@
+#include <receiver/nmea/epe.hpp>
+#include "helper.hpp"
+
+namespace receiver {
+namespace nmea {
+
+static bool parse_double_opt(std::string const& token, double& value) {
+    try {
+        value = std::stod(token);
+        return true;
+    } catch (...) {
+        value = 0;
+        return true;
+    }
+}
+
+EpeMessage::EpeMessage(std::string prefix, std::string payload, std::string checksum) NMEA_NOEXCEPT
+    : Message{prefix, payload, checksum},
+      mMsgVer{0.0},
+      mNorth{0.0},
+      mEast{0.0},
+      mDown{0.0},
+      m2D{0.0},
+      m3D{0.0} {}
+
+void EpeMessage::print() const NMEA_NOEXCEPT {
+    printf("[%5s]\n", prefix().c_str());
+    printf("  message version: %f\n", mMsgVer);
+    printf("  north error: %f\n", mNorth);
+    printf("  east error: %f\n", mEast);
+    printf("  down error: %f\n", mDown);
+    printf("  2D error: %f\n", m2D);
+    printf("  3D error: %f\n", m3D);
+}
+
+std::unique_ptr<Message> EpeMessage::parse(std::string prefix, std::string const& payload,
+                                           std::string checksum) {
+    // split payload by ','
+    auto tokens = split(payload, ',');
+
+    // check number of tokens
+    if (tokens.size() < 6) {
+#if RECEIVER_NMEA_DEBUG
+        printf("[--EPE] invalid number of tokens: %zu\n", tokens.size());
+#endif
+        return nullptr;
+    }
+
+    // parse
+    auto message = new EpeMessage(prefix, payload, checksum);
+    auto success = true;
+    success &= parse_double_opt(tokens[0], message->mMsgVer);
+    success &= parse_double_opt(tokens[1], message->mNorth);
+    success &= parse_double_opt(tokens[2], message->mEast);
+    success &= parse_double_opt(tokens[3], message->mDown);
+    success &= parse_double_opt(tokens[4], message->m2D);
+    success &= parse_double_opt(tokens[5], message->m3D);
+
+    if (success) {
+        return std::unique_ptr<EpeMessage>(message);
+    } else {
+#if RECEIVER_NMEA_DEBUG
+        printf("[--EPE] failed to parse message\n");
+#endif
+        delete message;
+        return nullptr;
+    }
+}
+
+}  // namespace nmea
+}  // namespace receiver

receiver/nmea/include/receiver/nmea/epe.hpp

@@ -0,0 +1,82 @@
+#pragma once
+#include <receiver/nmea/message.hpp>
+#include <receiver/nmea/types.hpp>
+
+#include <cmath>
+#include <memory>
+#include <utility/time.h>
+
+namespace receiver {
+namespace nmea {
+
+// Message specification
+// $PQTMEPE,<MsgVer>,<EPE_North>,<EPE_East>,<EPE_Down>,<EPE_2D>,<EPE_3D>*<Checksum>
+class EpeMessage final : public Message {
+public:
+    ~EpeMessage() override = default;
+
+    EpeMessage(EpeMessage const& other)
+        : Message(other), mMsgVer(other.mMsgVer), mNorth(other.mNorth), mEast(other.mEast),
+          mDown(other.mDown), m2D(other.m2D), m3D(other.m3D) {}
+    EpeMessage(EpeMessage&&)                 = delete;
+    EpeMessage& operator=(EpeMessage const&) = delete;
+    EpeMessage& operator=(EpeMessage&&)      = delete;
+
+    void print() const NMEA_NOEXCEPT override;
+
+    /// ----- EPE source messages -----
+
+    /// Get the estimated north error
+    NMEA_NODISCARD double north() const NMEA_NOEXCEPT { return mNorth; }
+
+    /// Get the estimated east error
+    NMEA_NODISCARD double east() const NMEA_NOEXCEPT { return mEast; }
+
+    /// Get the estimated down error
+    NMEA_NODISCARD double down() const NMEA_NOEXCEPT { return mDown; }
+
+    /// Get the estimated 2D position error
+    NMEA_NODISCARD double epe_2d() const NMEA_NOEXCEPT { return m2D; }
+
+    /// Get the estimated 3D position error
+    NMEA_NODISCARD double epe_3d() const NMEA_NOEXCEPT { return m3D; }
+
+    /// ----- GST conversions -----
+
+    /// Get the horizontal position error.
+    NMEA_NODISCARD double horizontal_position_error() const NMEA_NOEXCEPT { return m2D; }
+
+    /// Get semi-major axis.
+    NMEA_NODISCARD double semi_major() const NMEA_NOEXCEPT { return m2D / sqrt(2); }
+
+    /// Get semi-minor axis.
+    NMEA_NODISCARD double semi_minor() const NMEA_NOEXCEPT { return m2D / sqrt(2); }
+
+    /// Get the orientation of the semi-major axis.
+    NMEA_NODISCARD double orientation() const NMEA_NOEXCEPT { return 0; }
+
+    /// Get the vertical position error.
+    NMEA_NODISCARD double vertical_position_error() const NMEA_NOEXCEPT {
+        return sqrt((m3D * m3D) - (m2D * m2D));
+    }
+
+    // TODO(ehedpon) Implement conversion from north, east and down error to latitude and longitude.
+    // Also unclear if vertical error is sigma_v or if it needs to be converted from down
+
+    NMEA_NODISCARD static std::unique_ptr<Message>
+    parse(std::string prefix, std::string const& payload, std::string checksum);
+
+private:
+    NMEA_EXPLICIT EpeMessage(std::string prefix, std::string payload,
+                             std::string checksum) NMEA_NOEXCEPT;
+
+    double mMsgVer;
+    double mNorth;
+    double mEast;
+    double mDown;
+    double m2D;
+    double m3D;
+};
+
+}  // namespace nmea
+}  // namespace receiver

receiver/nmea/include/receiver/nmea/threaded_receiver.hpp

@@ -1,6 +1,7 @@
 #pragma once
 #include <receiver/nmea/gga.hpp>
 #include <receiver/nmea/gst.hpp>
+#include <receiver/nmea/epe.hpp>
 #include <receiver/nmea/receiver.hpp>
 #include <receiver/nmea/types.hpp>
 #include <receiver/nmea/vtg.hpp>
@@ -43,6 +44,10 @@ class ThreadedReceiver {
     /// @return A unique pointer to the message, or nullptr if no message has been received.
     NMEA_NODISCARD std::unique_ptr<GstMessage> gst() NMEA_NOEXCEPT;
 
+    /// Get the last received EPE message.
+    /// @return A unique pointer to the message, or nullptr if no message has been received.
+    NMEA_NODISCARD std::unique_ptr<EpeMessage> epe() NMEA_NOEXCEPT;
+
 protected:
     /// This function is called at the start of the receiver thread. It handles the blocking
     /// communication with the receiver.
@@ -60,6 +65,7 @@ class ThreadedReceiver {
     std::unique_ptr<GgaMessage> mGga;
     std::unique_ptr<VtgMessage> mVtg;
     std::unique_ptr<GstMessage> mGst;
+    std::unique_ptr<EpeMessage> mEpe;
 };
 
 }  // namespace nmea

receiver/nmea/parser.cpp

@@ -3,6 +3,7 @@
 #include "gst.hpp"
 #include "message.hpp"
 #include "vtg.hpp"
+#include "epe.hpp"
 
 #include <cstdio>
 
@@ -140,6 +141,14 @@ std::unique_ptr<Message> Parser::try_parse() NMEA_NOEXCEPT {
             return std::unique_ptr<ErrorMessage>(
                 new ErrorMessage(prefix, data_payload, data_checksum));
         }
+    } else if (prefix == "PQTMEPE") {
+        auto message = EpeMessage::parse(prefix, data_payload, data_checksum);
+        if (message) {
+            return message;
+        } else {
+            return std::unique_ptr<ErrorMessage>(
+                new ErrorMessage(prefix, data_payload, data_checksum));
+        }
     } else {
         return std::unique_ptr<UnsupportedMessage>(
             new UnsupportedMessage(prefix, data_payload, data_checksum));
@@ -231,7 +240,9 @@ std::string Parser::parse_prefix(uint8_t const* data, uint32_t length) const NME
         prefix += static_cast<char>(c);
     }
 
-    if (prefix.size() != 6) {
+    if (prefix.size() != 6 && prefix.size() != 8) {
+        // All NMEA messages have six characters in their prefix, eg $GPGST, except
+        // Quectel's EPE message, that has eight characters: $PQTMEPE.
         return "";
     }
 

receiver/nmea/threaded_receiver.cpp

@@ -22,7 +22,8 @@ ThreadedReceiver::ThreadedReceiver(
       mExportInterfaces(std::move(export_interfaces)),
       mGga(nullptr),
       mVtg(nullptr),
-      mGst(nullptr) {
+      mGst(nullptr),
+      mEpe(nullptr) {
     RNT_DEBUG("[rnt] created\n");
 }
 
@@ -95,6 +96,9 @@ void ThreadedReceiver::run() {
                         } else if (dynamic_cast<GstMessage*>(message.get())) {
                             mGst = std::unique_ptr<GstMessage>(
                                 static_cast<GstMessage*>(message.release()));
+                        } else if (dynamic_cast<EpeMessage*>(message.get())) {
+                            mEpe = std::unique_ptr<EpeMessage>(
+                                static_cast<EpeMessage*>(message.release()));
                         }
                     } else {
                         break;
@@ -156,5 +160,16 @@ std::unique_ptr<GstMessage> ThreadedReceiver::gst() NMEA_NOEXCEPT {
     return gst;
 }
 
+std::unique_ptr<EpeMessage> ThreadedReceiver::epe() NMEA_NOEXCEPT {
+    if (!mReceiver) return nullptr;
+    RNT_DEBUG("[rnt] lock (epe)\n");
+    std::lock_guard<std::mutex> lock(mMutex);
+
+    if (!mEpe) return nullptr;
+    auto epe = std::unique_ptr<EpeMessage>(new EpeMessage{*mEpe.get()});
+    RNT_DEBUG("[rnt] unlock (epe)\n");
+    return epe;
+}
+
 }  // namespace nmea
 }  // namespace receiver