From b60a38fed3e11d51184c12ee68f00ca0ae6d17a7 Mon Sep 17 00:00:00 2001 From: Christian Berger Date: Tue, 29 Jan 2019 20:46:49 +0100 Subject: [PATCH] * Added Grp10001,2,3,9 Signed-off-by: Christian Berger --- src/opendlv-device-gps-pos.cpp | 48 +++++++++++++++++++++++++++++++ src/pos-decoder.cpp | 52 ++++++++++++++++++++++++++++++---- src/pos-decoder.hpp | 10 ++++++- test/tests-pos-decoder.cpp | 42 +++++++++++++++++++++++++-- 4 files changed, 142 insertions(+), 10 deletions(-) diff --git a/src/opendlv-device-gps-pos.cpp b/src/opendlv-device-gps-pos.cpp index e779d30..b379c2b 100644 --- a/src/opendlv-device-gps-pos.cpp +++ b/src/opendlv-device-gps-pos.cpp @@ -111,6 +111,54 @@ int32_t main(int32_t argc, char **argv) { [&od4Session = od4, senderStamp = ID, VERBOSE](opendlv::device::gps::pos::Grp4Data m, const cluon::data::TimeStamp &sampleTime) { od4Session.send(m, sampleTime, 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 << " = " << v << '\n'; }, + []() {}); + std::cout << buffer.str() << " at " << sampleTime.seconds() << "." << sampleTime.microseconds() << std::endl; + } + }, + [&od4Session = od4, senderStamp = ID, VERBOSE](opendlv::device::gps::pos::Grp10001Data m, const cluon::data::TimeStamp &sampleTime) { + od4Session.send(m, sampleTime, 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 << " = " << v << '\n'; }, + []() {}); + std::cout << buffer.str() << " at " << sampleTime.seconds() << "." << sampleTime.microseconds() << std::endl; + } + }, + [&od4Session = od4, senderStamp = ID, VERBOSE](opendlv::device::gps::pos::Grp10002Data m, const cluon::data::TimeStamp &sampleTime) { + od4Session.send(m, sampleTime, 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 << " = " << v << '\n'; }, + []() {}); + std::cout << buffer.str() << " at " << sampleTime.seconds() << "." << sampleTime.microseconds() << std::endl; + } + }, + [&od4Session = od4, senderStamp = ID, VERBOSE](opendlv::device::gps::pos::Grp10003Data m, const cluon::data::TimeStamp &sampleTime) { + od4Session.send(m, sampleTime, 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 << " = " << v << '\n'; }, + []() {}); + std::cout << buffer.str() << " at " << sampleTime.seconds() << "." << sampleTime.microseconds() << std::endl; + } + }, + [&od4Session = od4, senderStamp = ID, VERBOSE](opendlv::device::gps::pos::Grp10009Data m, const cluon::data::TimeStamp &sampleTime) { + od4Session.send(m, sampleTime, senderStamp); + // Print values on console. if (VERBOSE) { std::stringstream buffer; diff --git a/src/pos-decoder.cpp b/src/pos-decoder.cpp index 0e8d533..e8b57b5 100644 --- a/src/pos-decoder.cpp +++ b/src/pos-decoder.cpp @@ -30,13 +30,21 @@ POSDecoder::POSDecoder(std::function delegateGrp1Data, std::function delegateGrp2Data, std::function delegateGrp3Data, - std::function delegateGrp4Data) noexcept + std::function delegateGrp4Data, + std::function d10001, + std::function d10002, + std::function d10003, + std::function d10009) noexcept : m_delegateLatitudeLongitude(std::move(delegateLatitudeLongitude)) , m_delegateHeading(std::move(delegateHeading)) , m_delegateGrp1Data(std::move(delegateGrp1Data)) , m_delegateGrp2Data(std::move(delegateGrp2Data)) , m_delegateGrp3Data(std::move(delegateGrp3Data)) - , m_delegateGrp4Data(std::move(delegateGrp4Data)) { + , m_delegateGrp4Data(std::move(delegateGrp4Data)) + , m_delegateGrp10001Data(std::move(d10001)) + , m_delegateGrp10002Data(std::move(d10002)) + , m_delegateGrp10003Data(std::move(d10003)) + , m_delegateGrp10009Data(std::move(d10009)) { m_dataBuffer = new uint8_t[POSDecoder::BUFFER_SIZE]; // Calculate offset between GPS and UTC. @@ -211,22 +219,54 @@ size_t POSDecoder::parseBuffer(uint8_t *buffer, const size_t size, std::chrono:: else if (POSDecoder::GRP10001 == groupNumber) { // Decode Applanix GRP10001. opendlv::device::gps::pos::Grp10001Data g10001Data{getGRP10001(b, messageSize)}; - (void)g10001Data; + + // Use timestamp from GPS if available. + if (1 == g10001Data.timeDistance().time1Type()) { + sampleTimeStamp = extractTimeDistance(g10001Data.timeDistance().time1()); + } + + if (nullptr != m_delegateGrp10001Data) { + m_delegateGrp10001Data(g10001Data, sampleTimeStamp); + } } else if (POSDecoder::GRP10002 == groupNumber) { // Decode Applanix GRP10002. opendlv::device::gps::pos::Grp10002Data g10002Data{getGRP10002(b, messageSize)}; - (void)g10002Data; + + // Use timestamp from GPS if available. + if (1 == g10002Data.timeDistance().time1Type()) { + sampleTimeStamp = extractTimeDistance(g10002Data.timeDistance().time1()); + } + + if (nullptr != m_delegateGrp10002Data) { + m_delegateGrp10002Data(g10002Data, sampleTimeStamp); + } } else if (POSDecoder::GRP10003 == groupNumber) { // Decode Applanix GRP10003. opendlv::device::gps::pos::Grp10003Data g10003Data{getGRP10003(b)}; - (void)g10003Data; + + // Use timestamp from GPS if available. + if (1 == g10003Data.timeDistance().time1Type()) { + sampleTimeStamp = extractTimeDistance(g10003Data.timeDistance().time1()); + } + + if (nullptr != m_delegateGrp10003Data) { + m_delegateGrp10003Data(g10003Data, sampleTimeStamp); + } } else if (POSDecoder::GRP10009 == groupNumber) { // Decode Applanix GRP10009. opendlv::device::gps::pos::Grp10009Data g10009Data{getGRP10009(b, messageSize)}; - (void)g10009Data; + + // Use timestamp from GPS if available. + if (1 == g10009Data.timeDistance().time1Type()) { + sampleTimeStamp = extractTimeDistance(g10009Data.timeDistance().time1()); + } + + if (nullptr != m_delegateGrp10009Data) { + m_delegateGrp10009Data(g10009Data, sampleTimeStamp); + } } // We have consumed the message, move offset accordingly. diff --git a/src/pos-decoder.hpp b/src/pos-decoder.hpp index 75cdcc5..bb5da5c 100644 --- a/src/pos-decoder.hpp +++ b/src/pos-decoder.hpp @@ -59,7 +59,11 @@ class POSDecoder { std::function d1, std::function d2, std::function d3, - std::function d4) noexcept; + std::function d4, + std::function d10001, + std::function d10002, + std::function d10003, + std::function d10009) noexcept; ~POSDecoder(); public: @@ -76,6 +80,10 @@ class POSDecoder { std::function m_delegateGrp2Data{}; std::function m_delegateGrp3Data{}; std::function m_delegateGrp4Data{}; + std::function m_delegateGrp10001Data{}; + std::function m_delegateGrp10002Data{}; + std::function m_delegateGrp10003Data{}; + std::function m_delegateGrp10009Data{}; private: opendlv::device::gps::pos::TimeDistance getTimeDistance(std::stringstream &buffer); diff --git a/test/tests-pos-decoder.cpp b/test/tests-pos-decoder.cpp index 494eafa..41d276c 100644 --- a/test/tests-pos-decoder.cpp +++ b/test/tests-pos-decoder.cpp @@ -12539,6 +12539,10 @@ TEST_CASE("Test POSDecoder with empty payload.") { bool grp2Called{false}; bool grp3Called{false}; bool grp4Called{false}; + bool grp10001Called{false}; + bool grp10002Called{false}; + bool grp10003Called{false}; + bool grp10009Called{false}; const std::string DATA; @@ -12548,7 +12552,11 @@ TEST_CASE("Test POSDecoder with empty payload.") { [&grp1Called](opendlv::device::gps::pos::Grp1Data, const cluon::data::TimeStamp &){ grp1Called = true; }, [&grp2Called](opendlv::device::gps::pos::Grp2Data, const cluon::data::TimeStamp &){ grp2Called = true; }, [&grp3Called](opendlv::device::gps::pos::Grp3Data, const cluon::data::TimeStamp &){ grp3Called = true; }, - [&grp4Called](opendlv::device::gps::pos::Grp4Data, const cluon::data::TimeStamp &){ grp4Called = true; } + [&grp4Called](opendlv::device::gps::pos::Grp4Data, const cluon::data::TimeStamp &){ grp4Called = true; }, + [&grp10001Called](opendlv::device::gps::pos::Grp10001Data, const cluon::data::TimeStamp &){ grp10001Called = true; }, + [&grp10002Called](opendlv::device::gps::pos::Grp10002Data, const cluon::data::TimeStamp &){ grp10002Called = true; }, + [&grp10003Called](opendlv::device::gps::pos::Grp10003Data, const cluon::data::TimeStamp &){ grp10003Called = true; }, + [&grp10009Called](opendlv::device::gps::pos::Grp10009Data, const cluon::data::TimeStamp &){ grp10009Called = true; } }; d.decode(DATA, std::chrono::system_clock::time_point()); @@ -12558,6 +12566,10 @@ TEST_CASE("Test POSDecoder with empty payload.") { REQUIRE(!grp2Called); REQUIRE(!grp3Called); REQUIRE(!grp4Called); + REQUIRE(!grp10001Called); + REQUIRE(!grp10002Called); + REQUIRE(!grp10003Called); + REQUIRE(!grp10009Called); } TEST_CASE("Test POSDecoder with faulty payload.") { @@ -12567,6 +12579,10 @@ TEST_CASE("Test POSDecoder with faulty payload.") { bool grp2Called{false}; bool grp3Called{false}; bool grp4Called{false}; + bool grp10001Called{false}; + bool grp10002Called{false}; + bool grp10003Called{false}; + bool grp10009Called{false}; const std::string DATA{"Hello World"}; @@ -12576,7 +12592,11 @@ TEST_CASE("Test POSDecoder with faulty payload.") { [&grp1Called](opendlv::device::gps::pos::Grp1Data, const cluon::data::TimeStamp &){ grp1Called = true; }, [&grp2Called](opendlv::device::gps::pos::Grp2Data, const cluon::data::TimeStamp &){ grp2Called = true; }, [&grp3Called](opendlv::device::gps::pos::Grp3Data, const cluon::data::TimeStamp &){ grp3Called = true; }, - [&grp4Called](opendlv::device::gps::pos::Grp4Data, const cluon::data::TimeStamp &){ grp4Called = true; } + [&grp4Called](opendlv::device::gps::pos::Grp4Data, const cluon::data::TimeStamp &){ grp4Called = true; }, + [&grp10001Called](opendlv::device::gps::pos::Grp10001Data, const cluon::data::TimeStamp &){ grp10001Called = true; }, + [&grp10002Called](opendlv::device::gps::pos::Grp10002Data, const cluon::data::TimeStamp &){ grp10002Called = true; }, + [&grp10003Called](opendlv::device::gps::pos::Grp10003Data, const cluon::data::TimeStamp &){ grp10003Called = true; }, + [&grp10009Called](opendlv::device::gps::pos::Grp10009Data, const cluon::data::TimeStamp &){ grp10009Called = true; } }; d.decode(DATA, std::chrono::system_clock::time_point()); @@ -12586,6 +12606,10 @@ TEST_CASE("Test POSDecoder with faulty payload.") { REQUIRE(!grp2Called); REQUIRE(!grp3Called); REQUIRE(!grp4Called); + REQUIRE(!grp10001Called); + REQUIRE(!grp10002Called); + REQUIRE(!grp10003Called); + REQUIRE(!grp10009Called); } TEST_CASE("Test POSDecoder with sample payload.") { @@ -12593,6 +12617,10 @@ TEST_CASE("Test POSDecoder with sample payload.") { bool grp2Called{false}; bool grp3Called{false}; bool grp4Called{false}; + bool grp10001Called{false}; + bool grp10002Called{false}; + bool grp10003Called{false}; + bool grp10009Called{false}; const std::string DATA(reinterpret_cast(POS_DUMP.data()), POS_DUMP.size()); std::vector > listOfGPS{}; @@ -12608,7 +12636,11 @@ TEST_CASE("Test POSDecoder with sample payload.") { [&grp1Called](opendlv::device::gps::pos::Grp1Data, const cluon::data::TimeStamp &){ grp1Called = true; }, [&grp2Called](opendlv::device::gps::pos::Grp2Data, const cluon::data::TimeStamp &){ grp2Called = true; }, [&grp3Called](opendlv::device::gps::pos::Grp3Data, const cluon::data::TimeStamp &){ grp3Called = true; }, - [&grp4Called](opendlv::device::gps::pos::Grp4Data, const cluon::data::TimeStamp &){ grp4Called = true; } + [&grp4Called](opendlv::device::gps::pos::Grp4Data, const cluon::data::TimeStamp &){ grp4Called = true; }, + [&grp10001Called](opendlv::device::gps::pos::Grp10001Data, const cluon::data::TimeStamp &){ grp10001Called = true; }, + [&grp10002Called](opendlv::device::gps::pos::Grp10002Data, const cluon::data::TimeStamp &){ grp10002Called = true; }, + [&grp10003Called](opendlv::device::gps::pos::Grp10003Data, const cluon::data::TimeStamp &){ grp10003Called = true; }, + [&grp10009Called](opendlv::device::gps::pos::Grp10009Data, const cluon::data::TimeStamp &){ grp10009Called = true; } }; uint32_t overallCounter{0}; @@ -12648,5 +12680,9 @@ TEST_CASE("Test POSDecoder with sample payload.") { REQUIRE(grp2Called); REQUIRE(!grp3Called); REQUIRE(grp4Called); + REQUIRE(grp10001Called); + REQUIRE(!grp10002Called); + REQUIRE(!grp10003Called); + REQUIRE(!grp10009Called); }