* supporting groundSpeed

Signed-off-by: Christian Berger <christian.berger@gu.se>
chalmers-revere · Feb 6, 2020 · 8ac9ebc · 8ac9ebc
1 parent 8dd9709
commit 8ac9ebc
Show file tree

Hide file tree

Showing 5 changed files with 87 additions and 19 deletions.
diff --git a/README.md b/README.md
@@ -36,13 +36,13 @@ the messages according to OpenDLV Standard Message Set into session 111 in
 Google Protobuf format, simply start it as follows:
 
 ```
-docker run --init --rm --net=host chalmersrevere/opendlv-device-gps-nmea-multi:v0.0.15 --nmea_ip=10.42.42.23 --nmea_port=9999 --cid=111 --verbose
+docker run --init --rm --net=host chalmersrevere/opendlv-device-gps-nmea-multi:v0.0.16 --nmea_ip=10.42.42.23 --nmea_port=9999 --cid=111 --verbose
 ```
 
 If you have an NMEA UDP producer, turn this microservice into a UDP-client listening to incoming UDP packets carrying raw NMEA messages:
 
 ```
-docker run --init --rm --net=host chalmersrevere/opendlv-device-gps-nmea-multi:v0.0.15 --udp --nmea_ip=0.0.0.0 --nmea_port=9999 --cid=111 --verbose
+docker run --init --rm --net=host chalmersrevere/opendlv-device-gps-nmea-multi:v0.0.16 --udp --nmea_ip=0.0.0.0 --nmea_port=9999 --cid=111 --verbose
 ```
 
 ## Build from sources on the example of Ubuntu 16.04 LTS

diff --git a/src/nmea-decoder.cpp b/src/nmea-decoder.cpp
@@ -25,10 +25,12 @@
 
 NMEADecoder::NMEADecoder(
     std::function<void(const double &latitude, const double &longitude, const std::chrono::system_clock::time_point &tp)> delegateLatitudeLongitude,
-    std::function<void(const float &heading, const std::chrono::system_clock::time_point &tp)> delegateHeading
+    std::function<void(const float &heading, const std::chrono::system_clock::time_point &tp)> delegateHeading,
+    std::function<void(const float &speed, const std::chrono::system_clock::time_point &tp)> delegateSpeed
     ) noexcept
     : m_delegateLatitudeLongitude(std::move(delegateLatitudeLongitude))
-    , m_delegateHeading(std::move(delegateHeading)) {
+    , m_delegateHeading(std::move(delegateHeading))
+    , m_delegateSpeed(std::move(delegateSpeed)) {
     m_buffer = new uint8_t[NMEADecoderConstants::BUFFER_SIZE];
 }
 
@@ -143,6 +145,11 @@ size_t NMEADecoder::parseBuffer(const uint8_t *buffer, const size_t size, std::c
                     if (nullptr != m_delegateHeading) {
                         m_delegateHeading(heading, timestamp);
                     }
+
+                    const float speed = static_cast<float>(std::stod(fields[7]) * 0.514444f);
+                    if (nullptr != m_delegateSpeed) {
+                        m_delegateSpeed(speed, timestamp);
+                    }
                 }
             }
             // Found CRLF; decode message.

diff --git a/src/nmea-decoder.hpp b/src/nmea-decoder.hpp
@@ -31,7 +31,8 @@ class NMEADecoder {
 
    public:
     NMEADecoder(std::function<void(const double &latitude, const double &longitude, const std::chrono::system_clock::time_point &tp)> delegateLatitudeLongitude,
-                std::function<void(const float &heading, const std::chrono::system_clock::time_point &tp)> delegateHeading) noexcept;
+                std::function<void(const float &heading, const std::chrono::system_clock::time_point &tp)> delegateHeading,
+                std::function<void(const float &speed, const std::chrono::system_clock::time_point &tp)> delegateSpeed) noexcept;
     ~NMEADecoder();
 
    public:
@@ -47,6 +48,7 @@ class NMEADecoder {
    private:
     std::function<void(const double &latitude, const double &longitude, const std::chrono::system_clock::time_point &tp)> m_delegateLatitudeLongitude{};
     std::function<void(const float &heading, const std::chrono::system_clock::time_point &tp)> m_delegateHeading{};
+    std::function<void(const float &speed, const std::chrono::system_clock::time_point &tp)> m_delegateSpeed{};
 };
 
 #endif

diff --git a/src/opendlv-device-gps-nmea.cpp b/src/opendlv-device-gps-nmea.cpp
@@ -68,6 +68,20 @@ int32_t main(int32_t argc, char **argv) {
                 m.northHeading(heading);
                 od4Session.send(m, cluon::time::convert(tp), senderStamp);
 
+                // Print values on console.
+                if (VERBOSE) {
+                    std::stringstream buffer;
+                    m.accept([](uint32_t, const std::string &, const std::string &) {},
+                             [&buffer](uint32_t, std::string &&, std::string &&n, auto v) { buffer << n << " = " << std::setprecision(9) << v << '\n'; },
+                             []() {});
+                    std::cout << buffer.str() << std::endl;
+                }
+            },
+            [&od4Session = od4, senderStamp = ID, VERBOSE](const float &speed, const std::chrono::system_clock::time_point &tp) {
+                opendlv::proxy::GroundSpeedReading m;
+                m.groundSpeed(speed);
+                od4Session.send(m, cluon::time::convert(tp), senderStamp);
+
                 // Print values on console.
                 if (VERBOSE) {
                     std::stringstream buffer;