Merge pull request 'implement relative timeshift' (#173) from time-service-relative-timeshift into develop

Reviewed-on: eive/fsfw#173
Reviewed-by: Marius Eggert <eggertm@irs.uni-stuttgart.de>

CHANGELOG.md

@@ -28,9 +28,12 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 - add CFDP subsystem ID
   https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/742
 - `PusTmZcWriter` now exposes API to set message counter field.
+- Relative timeshift in the PUS time service.
 
 ## Changed
 
+- The PUS time service now dumps the time before setting a new time and after having set the
+  time.
 - HK generation is now countdown based.
 - Bump ETL version to 20.35.14
   https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/748

src/fsfw/pus/Service9TimeManagement.cpp

@@ -2,9 +2,9 @@
 
 #include <cmath>
 
-#include "fsfw/events/EventManagerIF.h"
 #include "fsfw/pus/servicepackets/Service9Packets.h"
-#include "fsfw/serviceinterface/ServiceInterface.h"
+#include "fsfw/returnvalues/returnvalue.h"
+#include "fsfw/serialize/SerializeAdapter.h"
 #include "fsfw/timemanager/CCSDSTime.h"
 
 Service9TimeManagement::Service9TimeManagement(PsbParams params) : PusServiceBase(params) {
@@ -18,16 +18,53 @@ ReturnValue_t Service9TimeManagement::performService() { return returnvalue::OK;
 ReturnValue_t Service9TimeManagement::handleRequest(uint8_t subservice) {
   switch (subservice) {
     case Subservice::SET_TIME: {
-      return setTime();
+      reportCurrentTime(CLOCK_DUMP_BEFORE_SETTING_TIME);
+      ReturnValue_t result = setTime();
+      reportCurrentTime(CLOCK_DUMP_AFTER_SETTING_TIME);
+      return result;
     }
     case Subservice::DUMP_TIME: {
-      timeval newTime;
-      Clock::getClock_timeval(&newTime);
-      uint32_t subsecondMs =
-          static_cast<uint32_t>(std::floor(static_cast<double>(newTime.tv_usec) / 1000.0));
-      triggerEvent(CLOCK_DUMP, newTime.tv_sec, subsecondMs);
+      reportCurrentTime();
       return returnvalue::OK;
     }
+    case Subservice::RELATIVE_TIMESHIFT: {
+      timeval currentTime;
+      ReturnValue_t result = Clock::getClock(&currentTime);
+      if (result != returnvalue::OK) {
+        return result;
+      }
+      reportTime(CLOCK_DUMP_BEFORE_SETTING_TIME, currentTime);
+
+      if (currentPacket.getUserDataLen() != 8) {
+        return AcceptsTelecommandsIF::ILLEGAL_APPLICATION_DATA;
+      }
+      size_t deserLen = 8;
+      int64_t timeshiftNanos = 0;
+      result = SerializeAdapter::deSerialize(&timeshiftNanos, currentPacket.getUserData(),
+                                             &deserLen, SerializeIF::Endianness::NETWORK);
+      if (result != returnvalue::OK) {
+        return result;
+      }
+      bool positiveShift = true;
+      if (timeshiftNanos < 0) {
+        positiveShift = false;
+      }
+      timeval offset{};
+      offset.tv_sec = std::abs(timeshiftNanos) / NANOS_PER_SECOND;
+      offset.tv_usec = (std::abs(timeshiftNanos) % NANOS_PER_SECOND) / 1000;
+
+      timeval newTime;
+      if (positiveShift) {
+        newTime = currentTime + offset;
+      } else {
+        newTime = currentTime - offset;
+      }
+      result = Clock::setClock(&newTime);
+      if (result == returnvalue::OK) {
+        reportTime(CLOCK_DUMP_AFTER_SETTING_TIME, newTime);
+      }
+      return result;
+    }
     default:
       return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
   }
@@ -43,17 +80,20 @@ ReturnValue_t Service9TimeManagement::setTime() {
     return result;
   }
 
-  timeval time;
-  Clock::getClock_timeval(&time);
   result = Clock::setClock(&timeToSet);
-
-  if (result == returnvalue::OK) {
-    timeval newTime;
-    Clock::getClock_timeval(&newTime);
-    triggerEvent(CLOCK_SET, time.tv_sec, newTime.tv_sec);
-    return returnvalue::OK;
-  } else {
+  if (result != returnvalue::OK) {
     triggerEvent(CLOCK_SET_FAILURE, result, 0);
     return returnvalue::FAILED;
   }
+  return result;
+}
+
+void Service9TimeManagement::reportCurrentTime(Event event) {
+  timeval currentTime{};
+  Clock::getClock(&currentTime);
+  triggerEvent(event, currentTime.tv_sec, currentTime.tv_usec);
+}
+
+void Service9TimeManagement::reportTime(Event event, timeval time) {
+  triggerEvent(event, time.tv_sec, time.tv_usec);
 }

src/fsfw/pus/Service9TimeManagement.h

@@ -1,18 +1,25 @@
 #ifndef FSFW_PUS_SERVICE9TIMEMANAGEMENT_H_
 #define FSFW_PUS_SERVICE9TIMEMANAGEMENT_H_
 
+#include "fsfw/returnvalues/returnvalue.h"
 #include "fsfw/tmtcservices/PusServiceBase.h"
 
 class Service9TimeManagement : public PusServiceBase {
  public:
   static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PUS_SERVICE_9;
 
-  //!< Clock has been set. P1: old timeval seconds. P2: new timeval seconds.
+  static constexpr uint32_t NANOS_PER_SECOND = 1'000'000'000;
+
+  //!< [EXPORT] : [COMMENT] Clock has been set. P1: old timeval seconds. P2: new timeval seconds.
   static constexpr Event CLOCK_SET = MAKE_EVENT(0, severity::INFO);
-  //!< Clock dump event. P1: timeval seconds P2: timeval milliseconds.
-  static constexpr Event CLOCK_DUMP = MAKE_EVENT(1, severity::INFO);
-  //!< Clock could not be set. P1: Returncode.
+  //!< [EXPORT] : [COMMENT] Clock dump event. P1: timeval seconds P2: timeval milliseconds.
+  static constexpr Event CLOCK_DUMP_LEGACY = MAKE_EVENT(1, severity::INFO);
+  //!< [EXPORT] : [COMMENT] Clock could not be set. P1: Returncode.
   static constexpr Event CLOCK_SET_FAILURE = MAKE_EVENT(2, severity::LOW);
+  //!< [EXPORT] : [COMMENT] Clock dump event. P1: timeval seconds P2: timeval microseconds.
+  static constexpr Event CLOCK_DUMP = MAKE_EVENT(3, severity::INFO);
+  static constexpr Event CLOCK_DUMP_BEFORE_SETTING_TIME = MAKE_EVENT(4, severity::INFO);
+  static constexpr Event CLOCK_DUMP_AFTER_SETTING_TIME = MAKE_EVENT(5, severity::INFO);
 
   static constexpr uint8_t CLASS_ID = CLASS_ID::PUS_SERVICE_9;
 
@@ -30,12 +37,16 @@ class Service9TimeManagement : public PusServiceBase {
    */
   ReturnValue_t handleRequest(uint8_t subservice) override;
 
+  void reportCurrentTime(Event eventType = CLOCK_DUMP);
+  void reportTime(Event event, timeval time);
+
   virtual ReturnValue_t setTime();
 
  private:
   enum Subservice {
     SET_TIME = 128,  //!< [EXPORT] : [COMMAND] Time command in ASCII, CUC or CDS format
     DUMP_TIME = 129,
+    RELATIVE_TIMESHIFT = 130,
   };
 };