From a6d20de0a51b54cadab4549128dbec1e729755d3 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 7 Apr 2022 10:02:56 +0200
Subject: [PATCH] GPS linux controller update

---
 linux/devices/GPSHyperionLinuxController.cpp | 22 +++++++++++++++++++-
 linux/devices/GPSHyperionLinuxController.h   |  2 ++
 2 files changed, 23 insertions(+), 1 deletion(-)

diff --git a/linux/devices/GPSHyperionLinuxController.cpp b/linux/devices/GPSHyperionLinuxController.cpp
index a811fbc5..d696a188 100644
--- a/linux/devices/GPSHyperionLinuxController.cpp
+++ b/linux/devices/GPSHyperionLinuxController.cpp
@@ -9,13 +9,16 @@
 #include <fstream>
 #endif
 #include <cmath>
+#include <ctime>
 
 GPSHyperionLinuxController::GPSHyperionLinuxController(object_id_t objectId, object_id_t parentId,
                                                        bool debugHyperionGps)
     : ExtendedControllerBase(objectId, objects::NO_OBJECT),
       gpsSet(this),
       myGpsmm(GPSD_SHARED_MEMORY, nullptr),
-      debugHyperionGps(debugHyperionGps) {}
+      debugHyperionGps(debugHyperionGps) {
+  timeUpdateCd.resetTimer();
+}
 
 GPSHyperionLinuxController::~GPSHyperionLinuxController() {}
 
@@ -172,6 +175,20 @@ void GPSHyperionLinuxController::readGpsDataFromGpsd() {
   timeval time = {};
   time.tv_sec = gpsSet.unixSeconds.value;
   time.tv_usec = gps->fix.time.tv_nsec / 1000;
+  std::time_t t = std::time(nullptr);
+  if (time.tv_sec == t) {
+    timeIsConstantCounter++;
+  } else {
+    timeIsConstantCounter = 0;
+  }
+  // If the received time does not change anymore for whatever reason, do not set it here
+  // to avoid stale times. Also, don't do it too often often to avoid jumping times
+  if (timeIsConstantCounter < 20 and timeUpdateCd.hasTimedOut()) {
+    // Update the system time here for now. NTP seems to be unable to do so for whatever reason
+    settimeofday(&time, nullptr);
+    timeUpdateCd.resetTimer();
+  }
+
   Clock::TimeOfDay_t timeOfDay = {};
   Clock::convertTimevalToTimeOfDay(&time, &timeOfDay);
   gpsSet.year = timeOfDay.year;
@@ -192,6 +209,9 @@ void GPSHyperionLinuxController::readGpsDataFromGpsd() {
     std::cout << "Longitude: " << gps->fix.longitude << std::endl;
     std::cout << "Altitude(MSL): " << gps->fix.altMSL << std::endl;
     std::cout << "Speed(m/s): " << gps->fix.speed << std::endl;
+    std::time_t t = std::time(nullptr);
+    std::tm tm = *std::gmtime(&t);
+    std::cout << "C Time: " << std::put_time(&tm, "%c") << std::endl;
   }
 }
 #endif
diff --git a/linux/devices/GPSHyperionLinuxController.h b/linux/devices/GPSHyperionLinuxController.h
index f0e4e6e0..46acb5fc 100644
--- a/linux/devices/GPSHyperionLinuxController.h
+++ b/linux/devices/GPSHyperionLinuxController.h
@@ -51,6 +51,8 @@ class GPSHyperionLinuxController : public ExtendedControllerBase {
   bool modeCommanded = true;
   gpsmm myGpsmm;
   bool debugHyperionGps = false;
+  uint32_t timeIsConstantCounter = 0;
+  Countdown timeUpdateCd = Countdown(60);
 
   void readGpsDataFromGpsd();
 };