From efc36c926c14d0df504b9bddbcb85ad80abb841f Mon Sep 17 00:00:00 2001
From: angpre <angelo02prete@gmail.com>
Date: Mon, 29 May 2023 21:11:25 +0200
Subject: [PATCH] [UBXGPS] Added utc timestamp

---
 src/shared/sensors/UBXGPS/UBXGPSData.h     | 43 +++++++++++++++++++++-
 src/shared/sensors/UBXGPS/UBXGPSSerial.cpp |  8 ++++
 src/shared/sensors/UBXGPS/UBXGPSSpi.cpp    |  8 ++++
 src/tests/sensors/test-ubxgps-serial.cpp   |  7 +++-
 4 files changed, 62 insertions(+), 4 deletions(-)

diff --git a/src/shared/sensors/UBXGPS/UBXGPSData.h b/src/shared/sensors/UBXGPS/UBXGPSData.h
index ed734d258..43d0df2a9 100644
--- a/src/shared/sensors/UBXGPS/UBXGPSData.h
+++ b/src/shared/sensors/UBXGPS/UBXGPSData.h
@@ -27,13 +27,48 @@
 namespace Boardcore
 {
 
+/**
+ * @brief Structure to handle UBX UTC time.
+ * Nanoseconds range from -5000000 (5 ms) to 994999999 (~995 ms) and when
+ * negative the other fields have been rounded to the nearest hundredth of a
+ * second.
+ * Because of leap seconds, minutes can be a second longer or shorter, so
+ * seconds range from 0 to 60.
+ */
+struct UBXDateTime
+{
+    uint16_t year      = 0;  // Year (UTC) [y]
+    uint8_t month      = 0;  // Month, range 1..12 (UTC) [month]
+    uint8_t day        = 0;  // Day of month, range 1..31 (UTC) [d]
+    uint8_t hour       = 0;  // Hour of day, range 0..23 (UTC) [h]
+    uint8_t minute     = 0;  // Minute of hour, range 0..59 (UTC) [min]
+    uint8_t second     = 0;  // Seconds of minute, range 0..60 (UTC) [s]
+    int32_t nanosecond = 0;  // Fraction of second, range -1e9 .. 1e9 (UTC) [ns]
+    uint32_t accuracy  = 0;  // Time accuracy estimate (UTC) [ns]
+
+    static std::string header()
+    {
+        return "year,month,day,hour,minute,second,nanosecond,accuracy";
+    }
+
+    void print(std::ostream &os) const
+    {
+        os << year << "," << (int)month << "," << (int)day << "," << (int)hour
+           << "," << (int)minute << "," << (int)second << "," << nanosecond
+           << "," << accuracy;
+    }
+};
+
 struct UBXGPSData : public GPSData
 {
+    UBXDateTime ubxTime;
+
     static std::string header()
     {
         return "gpsTimestamp,latitude,longitude,height,velocityNorth,"
                "velocityEast,velocityDown,speed,track,positionDOP,satellites,"
-               "fix\n";
+               "fix," +
+               UBXDateTime::header() + "\n";
     }
 
     void print(std::ostream &os) const
@@ -41,7 +76,11 @@ struct UBXGPSData : public GPSData
         os << gpsTimestamp << "," << latitude << "," << longitude << ","
            << height << "," << velocityNorth << "," << velocityEast << ","
            << velocityDown << "," << speed << "," << track << "," << positionDOP
-           << "," << (int)satellites << "," << (int)fix << "\n";
+           << "," << (int)satellites << "," << (int)fix << ",";
+
+        ubxTime.print(os);
+
+        os << "\n";
     }
 };
 
diff --git a/src/shared/sensors/UBXGPS/UBXGPSSerial.cpp b/src/shared/sensors/UBXGPS/UBXGPSSerial.cpp
index 0cfb238df..957f0a04a 100644
--- a/src/shared/sensors/UBXGPS/UBXGPSSerial.cpp
+++ b/src/shared/sensors/UBXGPS/UBXGPSSerial.cpp
@@ -381,6 +381,14 @@ void UBXGPSSerial::run()
         threadSample.positionDOP   = (float)pvtP.pDOP / 1e2;
         threadSample.satellites    = pvtP.numSV;
         threadSample.fix           = pvtP.fixType;
+        threadSample.ubxTime       = {.year       = pvtP.year,
+                                      .month      = pvtP.month,
+                                      .day        = pvtP.day,
+                                      .hour       = pvtP.hour,
+                                      .minute     = pvtP.min,
+                                      .second     = pvtP.sec,
+                                      .nanosecond = pvtP.nano,
+                                      .accuracy   = pvtP.tAcc};
 
         StackLogger::getInstance().updateStack(THID_GPS);
     }
diff --git a/src/shared/sensors/UBXGPS/UBXGPSSpi.cpp b/src/shared/sensors/UBXGPS/UBXGPSSpi.cpp
index f01a045a9..d62ace27f 100644
--- a/src/shared/sensors/UBXGPS/UBXGPSSpi.cpp
+++ b/src/shared/sensors/UBXGPS/UBXGPSSpi.cpp
@@ -130,6 +130,14 @@ UBXGPSData UBXGPSSpi::sampleImpl()
     sample.positionDOP   = (float)pvtP.pDOP / 1e2;
     sample.satellites    = pvtP.numSV;
     sample.fix           = pvtP.fixType;
+    sample.ubxTime       = {.year       = pvtP.year,
+                            .month      = pvtP.month,
+                            .day        = pvtP.day,
+                            .hour       = pvtP.hour,
+                            .minute     = pvtP.min,
+                            .second     = pvtP.sec,
+                            .nanosecond = pvtP.nano,
+                            .accuracy   = pvtP.tAcc};
 
     return sample;
 }
diff --git a/src/tests/sensors/test-ubxgps-serial.cpp b/src/tests/sensors/test-ubxgps-serial.cpp
index 063bf6df8..722cbef73 100644
--- a/src/tests/sensors/test-ubxgps-serial.cpp
+++ b/src/tests/sensors/test-ubxgps-serial.cpp
@@ -81,10 +81,13 @@ int main()
         printf(
             "[gps] timestamp: % 4.3f, fix: %01d lat: % f lon: % f "
             "height: %4.1f nsat: %2d speed: %3.2f velN: % 3.2f velE: % 3.2f "
-            "track %3.1f\n",
+            "track %3.1f utc: %04u:%02u:%02u:%02u:%02u:%02u:%09ld\n",
             (float)dataGPS.gpsTimestamp / 1000000, dataGPS.fix,
             dataGPS.latitude, dataGPS.longitude, dataGPS.height,
             dataGPS.satellites, dataGPS.speed, dataGPS.velocityNorth,
-            dataGPS.velocityEast, dataGPS.track);
+            dataGPS.velocityEast, dataGPS.track, dataGPS.ubxTime.year,
+            dataGPS.ubxTime.month, dataGPS.ubxTime.day, dataGPS.ubxTime.hour,
+            dataGPS.ubxTime.minute, dataGPS.ubxTime.second,
+            dataGPS.ubxTime.nanosecond);
     }
 }
-- 
GitLab