From c8e376a625192f9ff46aaa0f493c54f73f0087b4 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Tue, 7 Feb 2023 10:03:51 +0100
Subject: [PATCH] add missing pool read guards

---
 linux/devices/GpsHyperionLinuxController.cpp | 61 +++++++++-----------
 linux/devices/GpsHyperionLinuxController.h   |  7 ++-
 2 files changed, 31 insertions(+), 37 deletions(-)

diff --git a/linux/devices/GpsHyperionLinuxController.cpp b/linux/devices/GpsHyperionLinuxController.cpp
index 6bbbcfb7..3c19ca72 100644
--- a/linux/devices/GpsHyperionLinuxController.cpp
+++ b/linux/devices/GpsHyperionLinuxController.cpp
@@ -44,6 +44,7 @@ ReturnValue_t GpsHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_
     }
   }
   if (mode == MODE_OFF) {
+    PoolReadGuard pg(&gpsSet);
     gpsSet.setValidity(false, true);
   }
   return returnvalue::OK;
@@ -217,7 +218,7 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
       modeCommanded = false;
     }
     gpsSet.setValidity(false, true);
-  } else if (gps.satellites_used > 0) {
+  } else if (gps.satellites_used > 0 && validFix) {
     gpsSet.setValidity(true, true);
   }
 
@@ -250,46 +251,20 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
     gpsSet.speed.setValid(false);
   }
 
-#if LIBGPS_VERSION_MINOR <= 17
-  gpsSet.unixSeconds.value = gps.fix.time;
-#else
-  gpsSet.unixSeconds.value = gps.fix.time.tv_sec;
-#endif
   timeval time = {};
-  time.tv_sec = gpsSet.unixSeconds.value;
 #if LIBGPS_VERSION_MINOR <= 17
-  double fractionalPart = gps.fix.time - std::floor(gps.fix.time);
+  gpsSet.unixSeconds.value = std::floor(gps.fix.time);
+  double fractionalPart = gps.fix.time - gpsSet.unixSeconds.value;
   time.tv_usec = fractionalPart * 1000.0 * 1000.0;
 #else
+  gpsSet.unixSeconds.value = gps.fix.time.tv_sec;
   time.tv_usec = gps.fix.time.tv_nsec / 1000;
 #endif
-  std::time_t t = std::time(nullptr);
-  if (time.tv_sec == t) {
-    timeIsConstantCounter++;
-  } else {
-    timeIsConstantCounter = 0;
-  }
-  if (timeInit and validFix) {
-    if (not utility::timeSanityCheck()) {
-#if OBSW_VERBOSE_LEVEL >= 1
-      time_t timeRaw = time.tv_sec;
-      std::tm *timeTm = std::gmtime(&timeRaw);
-      sif::info << "Setting invalid system time from GPS data directly: "
-                << std::put_time(timeTm, "%c %Z") << std::endl;
-#endif
-      // For some reason, the clock needs to be somewhat correct for NTP to work. Really dumb..
-      Clock::setClock(&time);
-    }
-    timeInit = false;
-  }
-  // 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.
-    // Further tests have shown that the time seems to be set by NTPD after some time..
-    // Clock::setClock(&time);
-    timeUpdateCd.resetTimer();
-  }
+  time.tv_sec = gpsSet.unixSeconds.value;
+  // If the time is totally wrong (e.g. year 2000 after system reset because we do not have a RTC
+  // and no time file available) we set it with the roughly valid time from the GPS.
+  // NTP might only work if the time difference between sys time and current time is not too large.
+  overwriteTimeIfNotSane(time, validFix);
 
   Clock::TimeOfDay_t timeOfDay = {};
   Clock::convertTimevalToTimeOfDay(&time, &timeOfDay);
@@ -325,3 +300,19 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
   }
   return returnvalue::OK;
 }
+
+void GpsHyperionLinuxController::overwriteTimeIfNotSane(timeval time, bool validFix) {
+  if (not timeInit and validFix) {
+    if (not utility::timeSanityCheck()) {
+#if OBSW_VERBOSE_LEVEL >= 1
+      time_t timeRaw = time.tv_sec;
+      std::tm *timeTm = std::gmtime(&timeRaw);
+      sif::info << "Overwriting invalid system time from GPS data directly: "
+                << std::put_time(timeTm, "%c %Z") << std::endl;
+#endif
+      // For some reason, the clock needs to be somewhat correct for NTP to work. Really dumb..
+      Clock::setClock(&time);
+    }
+    timeInit = true;
+  }
+}
diff --git a/linux/devices/GpsHyperionLinuxController.h b/linux/devices/GpsHyperionLinuxController.h
index d2dc91b6..5b0a84cd 100644
--- a/linux/devices/GpsHyperionLinuxController.h
+++ b/linux/devices/GpsHyperionLinuxController.h
@@ -59,17 +59,20 @@ class GpsHyperionLinuxController : public ExtendedControllerBase {
   ReadModes readMode = ReadModes::SOCKET;
   Countdown maxTimeToReachFix = Countdown(MAX_SECONDS_TO_REACH_FIX * 1000);
   bool modeCommanded = true;
-  bool timeInit = true;
+  bool timeInit = false;
   bool gpsNotOpenSwitch = true;
   bool gpsReadFailedSwitch = true;
   bool debugHyperionGps = false;
   int32_t noModeSetCntr = 0;
-  uint32_t timeIsConstantCounter = 0;
   Countdown timeUpdateCd = Countdown(60);
 
   // Returns true if the function should be called again or false if other
   // controller handling can be done.
   bool readGpsDataFromGpsd();
+  // If the time is totally wrong (e.g. year 2000 after system reset because we do not have a RTC)
+  // we set it with the roughly valid time from the GPS. For some reason, NTP might only work
+  // if the time difference between sys time and current time is not too large
+  void overwriteTimeIfNotSane(timeval time, bool validFix);
 };
 
 #endif /* MISSION_DEVICES_GPSHYPERIONHANDLER_H_ */