* Update timestamp handling for Applanix

Signed-off-by: Christian Berger <christian.berger@gu.se>
chalmers-revere · Dec 8, 2018 · 4bbfd3c · 4bbfd3c
1 parent e82bf3b
commit 4bbfd3c
Show file tree

Hide file tree

Showing 6 changed files with 65 additions and 26 deletions.
diff --git 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
@@ -44,32 +44,32 @@ 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) {
                     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() << 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) {
                     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() << 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
@@ -20,15 +20,40 @@
 
 #include <cmath>
 #include <cstring>
+#include <ctime>
 #include <array>
 #include <sstream>
 #include <string>
 
-POSDecoder::POSDecoder(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
+POSDecoder::POSDecoder(std::function<void(const double &latitude, const double &longitude, const cluon::data::TimeStamp &sampleTime)> delegateLatitudeLongitude,
+                         std::function<void(const float &heading, const cluon::data::TimeStamp &sampleTime)> 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(&currentTime, 0, sizeof(time_t));
+    std::time(&currentTime);
+
+    struct tm currentTimeBrokenDown;
+    std::memset(&currentTimeBrokenDown, 0, sizeof(struct tm));
+    gmtime_r(&currentTime, &currentTimeBrokenDown);
+
+    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<int64_t>(diff)/SECONDS_PER_WEEK)*SECONDS_PER_WEEK + GPS_DELTA_TO_UTC;
+    m_timeOffsetSinceGPSinMicroseconds *= static_cast<int64_t>(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<int64_t>(floor(g1Data.timeDistance().time1()))};
+                        int64_t microseconds{static_cast<int64_t>(floor((g1Data.timeDistance().time1()-seconds)* static_cast<int64_t>(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<float>(g1Data.heading()), timestamp);
+                        m_delegateHeading(static_cast<float>(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<opendlv::device::gps::pos::TimeDistance::TimeType>(timeTypes & 0x0F));
 //        timedist.setTime2Type(static_cast<opendlv::device::gps::pos::TimeDistance::TimeType>(timeTypes & 0xF0));

diff --git a/src/pos-decoder.hpp b/src/pos-decoder.hpp
@@ -54,8 +54,8 @@ class POSDecoder {
     POSDecoder &operator=(POSDecoder &&) = delete;
 
    public:
-    POSDecoder(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;
+    POSDecoder(std::function<void(const double &latitude, const double &longitude, const cluon::data::TimeStamp &sampleTime)> delegateLatitudeLongitude,
+                std::function<void(const float &heading, const cluon::data::TimeStamp &sampleTime)> 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<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 double &latitude, const double &longitude, const cluon::data::TimeStamp &sampleTime)> m_delegateLatitudeLongitude{};
+    std::function<void(const float &heading, const cluon::data::TimeStamp &sampleTime)> 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
@@ -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
@@ -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<float> 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);
         }
     };