2021-07-13 20:58:45 +02:00
|
|
|
#include "fsfw/pus/Service9TimeManagement.h"
|
2020-09-16 19:11:50 +02:00
|
|
|
|
2023-02-01 18:10:07 +01:00
|
|
|
#include <cmath>
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
#include "fsfw/pus/servicepackets/Service9Packets.h"
|
2024-04-08 13:14:20 +02:00
|
|
|
#include "fsfw/returnvalues/returnvalue.h"
|
2024-04-03 16:15:07 +02:00
|
|
|
#include "fsfw/serialize/SerializeAdapter.h"
|
2022-02-02 10:29:30 +01:00
|
|
|
#include "fsfw/timemanager/CCSDSTime.h"
|
2020-09-16 19:11:50 +02:00
|
|
|
|
2022-08-01 11:00:36 +02:00
|
|
|
Service9TimeManagement::Service9TimeManagement(PsbParams params) : PusServiceBase(params) {
|
|
|
|
params.name = "PUS 9 Time MGMT";
|
|
|
|
}
|
2020-09-16 19:11:50 +02:00
|
|
|
|
2022-07-20 22:21:15 +02:00
|
|
|
Service9TimeManagement::~Service9TimeManagement() = default;
|
2020-09-16 19:11:50 +02:00
|
|
|
|
2022-08-16 01:08:26 +02:00
|
|
|
ReturnValue_t Service9TimeManagement::performService() { return returnvalue::OK; }
|
2020-09-16 19:11:50 +02:00
|
|
|
|
|
|
|
ReturnValue_t Service9TimeManagement::handleRequest(uint8_t subservice) {
|
2022-02-02 10:29:30 +01:00
|
|
|
switch (subservice) {
|
2023-02-01 18:10:07 +01:00
|
|
|
case Subservice::SET_TIME: {
|
2024-04-08 13:14:20 +02:00
|
|
|
reportCurrentTime();
|
|
|
|
ReturnValue_t result = setTime();
|
|
|
|
reportCurrentTime();
|
|
|
|
return result;
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
2023-02-01 18:10:07 +01:00
|
|
|
case Subservice::DUMP_TIME: {
|
|
|
|
timeval newTime;
|
2024-04-03 16:19:40 +02:00
|
|
|
ReturnValue_t result = Clock::getClock_timeval(&newTime);
|
|
|
|
if (result != returnvalue::OK) {
|
|
|
|
return result;
|
|
|
|
}
|
2024-04-03 16:23:21 +02:00
|
|
|
triggerEvent(CLOCK_DUMP, newTime.tv_sec, newTime.tv_usec);
|
2023-02-01 18:10:07 +01:00
|
|
|
return returnvalue::OK;
|
|
|
|
}
|
2024-04-03 16:15:07 +02:00
|
|
|
case Subservice::RELATIVE_TIMESHIFT: {
|
|
|
|
timeval currentTime;
|
2024-04-03 16:19:40 +02:00
|
|
|
ReturnValue_t result = Clock::getClock_timeval(¤tTime);
|
|
|
|
if (result != returnvalue::OK) {
|
|
|
|
return result;
|
|
|
|
}
|
2024-04-08 13:14:20 +02:00
|
|
|
triggerEvent(CLOCK_DUMP, currentTime.tv_sec, currentTime.tv_usec);
|
2024-04-03 16:15:07 +02:00
|
|
|
|
|
|
|
if (currentPacket.getUserDataLen() != 8) {
|
|
|
|
return AcceptsTelecommandsIF::ILLEGAL_APPLICATION_DATA;
|
|
|
|
}
|
|
|
|
size_t deserLen = 8;
|
|
|
|
int64_t timeshiftNanos = 0;
|
2024-04-03 16:19:40 +02:00
|
|
|
result = SerializeAdapter::deSerialize(×hiftNanos, currentPacket.getUserData(),
|
|
|
|
&deserLen, SerializeIF::Endianness::NETWORK);
|
|
|
|
if (result != returnvalue::OK) {
|
|
|
|
return result;
|
|
|
|
}
|
2024-04-03 16:15:07 +02:00
|
|
|
bool positiveShift = true;
|
|
|
|
if (timeshiftNanos < 0) {
|
|
|
|
positiveShift = false;
|
|
|
|
}
|
|
|
|
timeval offset{};
|
2024-04-08 10:34:28 +02:00
|
|
|
offset.tv_sec = std::abs(timeshiftNanos) / NANOS_PER_SECOND;
|
|
|
|
offset.tv_usec = (std::abs(timeshiftNanos) % NANOS_PER_SECOND) / 1000;
|
2024-04-03 16:15:07 +02:00
|
|
|
|
|
|
|
timeval newTime;
|
|
|
|
if (positiveShift) {
|
|
|
|
newTime = currentTime + offset;
|
|
|
|
} else {
|
|
|
|
newTime = currentTime - offset;
|
|
|
|
}
|
2024-04-03 16:23:21 +02:00
|
|
|
result = Clock::setClock(&newTime);
|
|
|
|
if (result == returnvalue::OK) {
|
2024-04-08 13:14:20 +02:00
|
|
|
reportTime(newTime);
|
2024-04-03 16:23:21 +02:00
|
|
|
}
|
|
|
|
return result;
|
2024-04-03 16:15:07 +02:00
|
|
|
}
|
2022-02-02 10:29:30 +01:00
|
|
|
default:
|
|
|
|
return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
|
|
|
|
}
|
2020-09-16 19:11:50 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
ReturnValue_t Service9TimeManagement::setTime() {
|
2022-02-02 10:29:30 +01:00
|
|
|
Clock::TimeOfDay_t timeToSet;
|
2022-07-21 11:34:11 +02:00
|
|
|
TimePacket timePacket(currentPacket.getUserData(), currentPacket.getUserDataLen());
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t result =
|
|
|
|
CCSDSTime::convertFromCcsds(&timeToSet, timePacket.getTime(), timePacket.getTimeSize());
|
2022-08-16 01:08:26 +02:00
|
|
|
if (result != returnvalue::OK) {
|
2022-02-02 10:29:30 +01:00
|
|
|
triggerEvent(CLOCK_SET_FAILURE, result, 0);
|
|
|
|
return result;
|
|
|
|
}
|
|
|
|
|
|
|
|
result = Clock::setClock(&timeToSet);
|
2024-04-08 13:14:20 +02:00
|
|
|
if (result != returnvalue::OK) {
|
2022-02-02 10:29:30 +01:00
|
|
|
triggerEvent(CLOCK_SET_FAILURE, result, 0);
|
2022-08-16 01:08:26 +02:00
|
|
|
return returnvalue::FAILED;
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
2024-04-08 13:14:20 +02:00
|
|
|
return result;
|
|
|
|
}
|
|
|
|
|
|
|
|
void Service9TimeManagement::reportCurrentTime() {
|
|
|
|
timeval currentTime{};
|
|
|
|
Clock::getClock_timeval(¤tTime);
|
|
|
|
triggerEvent(CLOCK_DUMP, currentTime.tv_sec, currentTime.tv_usec);
|
|
|
|
}
|
|
|
|
|
|
|
|
void Service9TimeManagement::reportTime(timeval time) {
|
|
|
|
triggerEvent(CLOCK_DUMP, time.tv_sec, time.tv_usec);
|
2020-09-16 19:11:50 +02:00
|
|
|
}
|