diff --git a/src/fsfw/pus/Service9TimeManagement.cpp b/src/fsfw/pus/Service9TimeManagement.cpp index fb32f60e..cf7458cc 100644 --- a/src/fsfw/pus/Service9TimeManagement.cpp +++ b/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(¤tTime); + + if (currentPacket.getUserDataLen() != 8) { + return AcceptsTelecommandsIF::ILLEGAL_APPLICATION_DATA; + } + size_t deserLen = 8; + int64_t timeshiftNanos = 0; + SerializeAdapter::deSerialize(×hiftNanos, 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; } diff --git a/src/fsfw/pus/Service9TimeManagement.h b/src/fsfw/pus/Service9TimeManagement.h index 556f3df3..508373ac 100644 --- a/src/fsfw/pus/Service9TimeManagement.h +++ b/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, }; };