src/fsfw/pus/Service9TimeManagement.cpp

@@ -4,6 +4,7 @@
 
 #include "fsfw/events/EventManagerIF.h"
 #include "fsfw/pus/servicepackets/Service9Packets.h"
+#include "fsfw/serialize/SerializeAdapter.h"
 #include "fsfw/serviceinterface/ServiceInterface.h"
 #include "fsfw/timemanager/CCSDSTime.h"
 
@@ -28,6 +29,34 @@ ReturnValue_t Service9TimeManagement::handleRequest(uint8_t subservice) {
       triggerEvent(CLOCK_DUMP, newTime.tv_sec, subsecondMs);
       return returnvalue::OK;
     }
+    case Subservice::RELATIVE_TIMESHIFT: {
+      timeval currentTime;
+      Clock::getClock_timeval(&currentTime);
+
+      if (currentPacket.getUserDataLen() != 8) {
+        return AcceptsTelecommandsIF::ILLEGAL_APPLICATION_DATA;
+      }
+      size_t deserLen = 8;
+      int64_t timeshiftNanos = 0;
+      SerializeAdapter::deSerialize(&timeshiftNanos, currentPacket.getUserData(), &deserLen,
+                                    SerializeIF::Endianness::NETWORK);
+      bool positiveShift = true;
+      if (timeshiftNanos < 0) {
+        positiveShift = false;
+      }
+      timeval offset{};
+      offset.tv_sec = std::abs(timeshiftNanos) / 1000000000;
+      offset.tv_usec = (std::abs(timeshiftNanos) % 1000000000) / 1000;
+
+      timeval newTime;
+      if (positiveShift) {
+        newTime = currentTime + offset;
+      } else {
+        newTime = currentTime - offset;
+      }
+      Clock::setClock(&newTime);
+      return returnvalue::OK;
+    }
     default:
       return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
   }

src/fsfw/pus/Service9TimeManagement.h

@@ -36,6 +36,7 @@ class Service9TimeManagement : public PusServiceBase {
   enum Subservice {
     SET_TIME = 128,  //!< [EXPORT] : [COMMAND] Time command in ASCII, CUC or CDS format
     DUMP_TIME = 129,
+    RELATIVE_TIMESHIFT = 130,
   };
 };