From 4bbfd3c984bcdfbc2ddb170a3a2fbda89445547b Mon Sep 17 00:00:00 2001 From: Christian Berger Date: Sat, 8 Dec 2018 21:51:11 +0100 Subject: [PATCH] * Update timestamp handling for Applanix Signed-off-by: Christian Berger --- README.md | 2 +- src/opendlv-device-gps-pos.cpp | 12 ++++----- src/pos-decoder.cpp | 48 +++++++++++++++++++++++++++++----- src/pos-decoder.hpp | 9 ++++--- src/pos-message-set.odvd | 8 +++--- test/tests-pos-decoder.cpp | 12 ++++----- 6 files changed, 65 insertions(+), 26 deletions(-) diff --git a/README.md b/README.md index c14e450..cd248d9 100644 --- a/README.md +++ b/README.md @@ -35,7 +35,7 @@ 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-pos-multi:v0.0.7 --pos_ip=192.168.1.77 --pos_port=5602 --cid=111 --verbose +docker run --init --rm --net=host chalmersrevere/opendlv-device-gps-pos-multi:v0.0.8 --pos_ip=192.168.1.77 --pos_port=5602 --cid=111 --verbose ``` ## Build from sources on the example of Ubuntu 16.04 LTS diff --git a/src/opendlv-device-gps-pos.cpp b/src/opendlv-device-gps-pos.cpp index de75967..fed249f 100644 --- a/src/opendlv-device-gps-pos.cpp +++ b/src/opendlv-device-gps-pos.cpp @@ -44,10 +44,10 @@ int32_t main(int32_t argc, char **argv) { }; POSDecoder posDecoder{ - [&od4Session = od4, senderStamp = ID, VERBOSE](const double &latitude, const double &longitude, const std::chrono::system_clock::time_point &tp) { + [&od4Session = od4, senderStamp = ID, VERBOSE](const double &latitude, const double &longitude, const cluon::data::TimeStamp &sampleTime) { opendlv::proxy::GeodeticWgs84Reading m; m.latitude(latitude).longitude(longitude); - od4Session.send(m, cluon::time::convert(tp), senderStamp); + od4Session.send(m, sampleTime, senderStamp); // Print values on console. if (VERBOSE) { @@ -55,13 +55,13 @@ int32_t main(int32_t argc, char **argv) { 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() << std::endl; + std::cout << buffer.str() << " at " << sampleTime.seconds() << "." << sampleTime.microseconds() << std::endl; } }, - [&od4Session = od4, senderStamp = ID, VERBOSE](const float &heading, const std::chrono::system_clock::time_point &tp) { + [&od4Session = od4, senderStamp = ID, VERBOSE](const float &heading, const cluon::data::TimeStamp &sampleTime) { opendlv::proxy::GeodeticHeadingReading m; m.northHeading(heading); - od4Session.send(m, cluon::time::convert(tp), senderStamp); + od4Session.send(m, sampleTime, senderStamp); // Print values on console. if (VERBOSE) { @@ -69,7 +69,7 @@ int32_t main(int32_t argc, char **argv) { 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() << std::endl; + std::cout << buffer.str() << " at " << sampleTime.seconds() << "." << sampleTime.microseconds() << std::endl; } } }; diff --git a/src/pos-decoder.cpp b/src/pos-decoder.cpp index 2d80163..8168169 100644 --- a/src/pos-decoder.cpp +++ b/src/pos-decoder.cpp @@ -20,15 +20,40 @@ #include #include +#include #include #include #include -POSDecoder::POSDecoder(std::function delegateLatitudeLongitude, - std::function delegateHeading) noexcept +POSDecoder::POSDecoder(std::function delegateLatitudeLongitude, + std::function delegateHeading) noexcept : m_delegateLatitudeLongitude(std::move(delegateLatitudeLongitude)) , m_delegateHeading(std::move(delegateHeading)) { m_dataBuffer = new uint8_t[POSDecoder::BUFFER_SIZE]; + + // Calculate offset between GPS and UTC. + auto createTimeFromYYYYMMDD = [](int year, int month, int day){ + struct tm tm; + std::memset(&tm, 0, sizeof(struct tm)); + tm.tm_year = year - 1900; + tm.tm_mon = month - 1; + tm.tm_mday = day; + return mktime(&tm); + }; + + time_t currentTime; + std::memset(¤tTime, 0, sizeof(time_t)); + std::time(¤tTime); + + struct tm currentTimeBrokenDown; + std::memset(¤tTimeBrokenDown, 0, sizeof(struct tm)); + gmtime_r(¤tTime, ¤tTimeBrokenDown); + + constexpr const int64_t GPS_DELTA_TO_UTC{315964800}; + constexpr const int64_t SECONDS_PER_WEEK{60*60*24*7}; + const double diff = difftime(createTimeFromYYYYMMDD(currentTimeBrokenDown.tm_year+1900, currentTimeBrokenDown.tm_mon+1, currentTimeBrokenDown.tm_mday), createTimeFromYYYYMMDD(1980, 1, 6)); + m_timeOffsetSinceGPSinMicroseconds = (static_cast(diff)/SECONDS_PER_WEEK)*SECONDS_PER_WEEK + GPS_DELTA_TO_UTC; + m_timeOffsetSinceGPSinMicroseconds *= static_cast(1000*1000); } POSDecoder::~POSDecoder() { @@ -63,7 +88,7 @@ void POSDecoder::decode(const std::string &data, std::chrono::system_clock::time } size_t POSDecoder::parseBuffer(uint8_t *buffer, const size_t size, std::chrono::system_clock::time_point &&tp) { - const std::chrono::system_clock::time_point timestamp{std::move(tp)}; + cluon::data::TimeStamp sampleTimeStamp{cluon::time::convert(std::move(tp))}; size_t offset{0}; while (true) { @@ -110,11 +135,22 @@ size_t POSDecoder::parseBuffer(uint8_t *buffer, const size_t size, std::chrono:: // Decode Applanix GRP1. opendlv::device::gps::pos::Grp1Data g1Data{getGRP1(b)}; + // Use timestamp from GPS if available. + if (1 == g1Data.timeDistance().time1Type()) { + int64_t seconds{static_cast(floor(g1Data.timeDistance().time1()))}; + int64_t microseconds{static_cast(floor((g1Data.timeDistance().time1()-seconds)* static_cast(1000*1000)))}; + + cluon::data::TimeStamp relativeTimeStamp; + relativeTimeStamp.seconds(seconds).microseconds(microseconds); + + sampleTimeStamp = cluon::time::fromMicroseconds(m_timeOffsetSinceGPSinMicroseconds + cluon::time::toMicroseconds(relativeTimeStamp)); + } + if (nullptr != m_delegateLatitudeLongitude) { - m_delegateLatitudeLongitude(g1Data.lat(), g1Data.lon(), timestamp); + m_delegateLatitudeLongitude(g1Data.lat(), g1Data.lon(), sampleTimeStamp); } if (nullptr != m_delegateHeading) { - m_delegateHeading(static_cast(g1Data.heading()), timestamp); + m_delegateHeading(static_cast(g1Data.heading()), sampleTimeStamp); } } else if (POSDecoder::GRP2 == groupNumber) { @@ -184,7 +220,7 @@ opendlv::device::gps::pos::TimeDistance POSDecoder::getTimeDistance(std::strings buffer.read((char *)(&(timeTypes)), sizeof(timeTypes)); buffer.read((char *)(&(distanceType)), sizeof(distanceType)); - timedist.time1(time1).time2(time2).distanceTag(distanceTag); + timedist.time1(time1).time2(time2).distanceTag(distanceTag).time1Type((timeTypes&0x7)).time2Type((timeTypes>>3)); // TODO: Add enum support to cluon-msc. // timedist.setTime1Type(static_cast(timeTypes & 0x0F)); // timedist.setTime2Type(static_cast(timeTypes & 0xF0)); diff --git a/src/pos-decoder.hpp b/src/pos-decoder.hpp index 52b04c7..bc16a8f 100644 --- a/src/pos-decoder.hpp +++ b/src/pos-decoder.hpp @@ -54,8 +54,8 @@ class POSDecoder { POSDecoder &operator=(POSDecoder &&) = delete; public: - POSDecoder(std::function delegateLatitudeLongitude, - std::function delegateHeading) noexcept; + POSDecoder(std::function delegateLatitudeLongitude, + std::function delegateHeading) noexcept; ~POSDecoder(); public: @@ -65,8 +65,8 @@ class POSDecoder { size_t parseBuffer(uint8_t *buffer, const size_t size, std::chrono::system_clock::time_point &&tp); private: - std::function m_delegateLatitudeLongitude{}; - std::function m_delegateHeading{}; + std::function m_delegateLatitudeLongitude{}; + std::function m_delegateHeading{}; private: opendlv::device::gps::pos::TimeDistance getTimeDistance(std::stringstream &buffer); @@ -83,6 +83,7 @@ class POSDecoder { private: uint8_t *m_dataBuffer{nullptr}; size_t m_size{0}; + int64_t m_timeOffsetSinceGPSinMicroseconds{0}; }; #endif diff --git a/src/pos-message-set.odvd b/src/pos-message-set.odvd index c9db2a0..7813c49 100644 --- a/src/pos-message-set.odvd +++ b/src/pos-message-set.odvd @@ -38,10 +38,12 @@ message opendlv.device.gps.pos.TimeDistance [id = 532] { double time1 [id = 1]; double time2 [id = 2]; double distanceTag [id = 3]; + uint8 time1Type [id = 4]; + uint8 time2Type [id = 5]; /* - TimeType time1Type [id = 4]; - TimeType time2Type [id = 5]; - DistanceType distanceType [id = 6]; + TimeType time1Type [id = 6]; + TimeType time2Type [id = 7]; + DistanceType distanceType [id = 8]; */ } diff --git a/test/tests-pos-decoder.cpp b/test/tests-pos-decoder.cpp index 676fe42..388fa4e 100644 --- a/test/tests-pos-decoder.cpp +++ b/test/tests-pos-decoder.cpp @@ -12539,8 +12539,8 @@ TEST_CASE("Test POSDecoder with empty payload.") { const std::string DATA; POSDecoder d{ - [&latLonCalled](const double&, const double&, const std::chrono::system_clock::time_point &){ latLonCalled = true; }, - [&headingCalled](const float&, const std::chrono::system_clock::time_point &){ headingCalled = true; } + [&latLonCalled](const double&, const double&, const cluon::data::TimeStamp &){ latLonCalled = true; }, + [&headingCalled](const float&, const cluon::data::TimeStamp &){ headingCalled = true; } }; d.decode(DATA, std::chrono::system_clock::time_point()); @@ -12555,8 +12555,8 @@ TEST_CASE("Test POSDecoder with faulty payload.") { const std::string DATA{"Hello World"}; POSDecoder d{ - [&latLonCalled](const double&, const double&, const std::chrono::system_clock::time_point &){ latLonCalled = true; }, - [&headingCalled](const float&, const std::chrono::system_clock::time_point &){ headingCalled = true; } + [&latLonCalled](const double&, const double&, const cluon::data::TimeStamp &){ latLonCalled = true; }, + [&headingCalled](const float&, const cluon::data::TimeStamp &){ headingCalled = true; } }; d.decode(DATA, std::chrono::system_clock::time_point()); @@ -12571,10 +12571,10 @@ TEST_CASE("Test POSDecoder with sample payload.") { std::vector listOfHeadings{}; POSDecoder d{ - [&listOfGPS](const double &lat, const double &lon, const std::chrono::system_clock::time_point &) { + [&listOfGPS](const double &lat, const double &lon, const cluon::data::TimeStamp &) { listOfGPS.push_back(std::make_pair(lat, lon)); }, - [&listOfHeadings](const float &h, const std::chrono::system_clock::time_point &) { + [&listOfHeadings](const float &h, const cluon::data::TimeStamp &) { listOfHeadings.push_back(h); } };