2021-07-13 20:58:45 +02:00
|
|
|
#include "fsfw/pus/Service8FunctionManagement.h"
|
2020-08-13 20:53:35 +02:00
|
|
|
|
2021-07-13 20:58:45 +02:00
|
|
|
#include "fsfw/action/HasActionsIF.h"
|
|
|
|
#include "fsfw/devicehandlers/DeviceHandlerIF.h"
|
2022-02-02 10:29:30 +01:00
|
|
|
#include "fsfw/objectmanager/ObjectManager.h"
|
|
|
|
#include "fsfw/objectmanager/SystemObjectIF.h"
|
|
|
|
#include "fsfw/pus/servicepackets/Service8Packets.h"
|
2021-07-13 20:58:45 +02:00
|
|
|
#include "fsfw/serialize/SerializeAdapter.h"
|
|
|
|
#include "fsfw/serviceinterface/ServiceInterface.h"
|
2020-08-13 20:53:35 +02:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
Service8FunctionManagement::Service8FunctionManagement(object_id_t objectId, uint16_t apid,
|
|
|
|
uint8_t serviceId,
|
|
|
|
uint8_t numParallelCommands,
|
|
|
|
uint16_t commandTimeoutSeconds)
|
|
|
|
: CommandingServiceBase(objectId, apid, serviceId, numParallelCommands, commandTimeoutSeconds) {
|
|
|
|
}
|
2020-08-13 20:53:35 +02:00
|
|
|
|
|
|
|
Service8FunctionManagement::~Service8FunctionManagement() {}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t Service8FunctionManagement::isValidSubservice(uint8_t subservice) {
|
|
|
|
switch (static_cast<Subservice>(subservice)) {
|
|
|
|
case Subservice::COMMAND_DIRECT_COMMANDING:
|
|
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
|
|
default:
|
|
|
|
return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
|
|
|
|
}
|
2020-08-13 20:53:35 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t Service8FunctionManagement::getMessageQueueAndObject(uint8_t subservice,
|
|
|
|
const uint8_t* tcData,
|
|
|
|
size_t tcDataLen,
|
|
|
|
MessageQueueId_t* id,
|
|
|
|
object_id_t* objectId) {
|
|
|
|
if (tcDataLen < sizeof(object_id_t)) {
|
|
|
|
return CommandingServiceBase::INVALID_TC;
|
|
|
|
}
|
|
|
|
// Can't fail, size was checked before
|
|
|
|
SerializeAdapter::deSerialize(objectId, &tcData, &tcDataLen, SerializeIF::Endianness::BIG);
|
|
|
|
|
|
|
|
return checkInterfaceAndAcquireMessageQueue(id, objectId);
|
2020-08-13 20:53:35 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
ReturnValue_t Service8FunctionManagement::checkInterfaceAndAcquireMessageQueue(
|
2022-02-02 10:29:30 +01:00
|
|
|
MessageQueueId_t* messageQueueToSet, object_id_t* objectId) {
|
|
|
|
// check HasActionIF property of target
|
|
|
|
HasActionsIF* possibleTarget = ObjectManager::instance()->get<HasActionsIF>(*objectId);
|
|
|
|
if (possibleTarget == nullptr) {
|
|
|
|
return CommandingServiceBase::INVALID_OBJECT;
|
|
|
|
}
|
|
|
|
*messageQueueToSet = possibleTarget->getCommandQueue();
|
|
|
|
return HasReturnvaluesIF::RETURN_OK;
|
2020-08-13 20:53:35 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t Service8FunctionManagement::prepareCommand(CommandMessage* message,
|
|
|
|
uint8_t subservice, const uint8_t* tcData,
|
|
|
|
size_t tcDataLen, uint32_t* state,
|
|
|
|
object_id_t objectId) {
|
|
|
|
return prepareDirectCommand(message, tcData, tcDataLen);
|
2020-08-13 20:53:35 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t Service8FunctionManagement::prepareDirectCommand(CommandMessage* message,
|
|
|
|
const uint8_t* tcData,
|
|
|
|
size_t tcDataLen) {
|
|
|
|
if (message == nullptr) {
|
|
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
|
|
}
|
|
|
|
if (tcDataLen < sizeof(object_id_t) + sizeof(ActionId_t)) {
|
2021-01-03 14:16:52 +01:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::debug << "Service8FunctionManagement::prepareDirectCommand:"
|
|
|
|
<< " TC size smaller thant minimum size of direct command." << std::endl;
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2022-02-02 10:29:30 +01:00
|
|
|
return CommandingServiceBase::INVALID_TC;
|
|
|
|
}
|
2020-08-13 20:53:35 +02:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
// Create direct command instance by extracting data from Telecommand
|
|
|
|
DirectCommand command(tcData, tcDataLen);
|
2020-08-13 20:53:35 +02:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
// store additional parameters into the IPC Store
|
|
|
|
store_address_t parameterAddress;
|
|
|
|
ReturnValue_t result =
|
2022-07-20 22:21:15 +02:00
|
|
|
ipcStore->addData(¶meterAddress, command.getParameters(), command.getParametersSize());
|
2020-08-13 20:53:35 +02:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
// setCommand expects a Command Message, an Action ID and a store adress
|
|
|
|
// pointing to additional parameters
|
|
|
|
ActionMessage::setCommand(message, command.getActionId(), parameterAddress);
|
|
|
|
return result;
|
2020-08-13 20:53:35 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t Service8FunctionManagement::handleReply(const CommandMessage* reply,
|
|
|
|
Command_t previousCommand, uint32_t* state,
|
|
|
|
CommandMessage* optionalNextCommand,
|
|
|
|
object_id_t objectId, bool* isStep) {
|
|
|
|
Command_t replyId = reply->getCommand();
|
|
|
|
ReturnValue_t result = HasReturnvaluesIF::RETURN_FAILED;
|
|
|
|
ActionId_t actionId = ActionMessage::getActionId(reply);
|
|
|
|
ReturnValue_t returnCode = ActionMessage::getReturnCode(reply);
|
|
|
|
|
|
|
|
switch (replyId) {
|
|
|
|
case ActionMessage::COMPLETION_SUCCESS: {
|
|
|
|
DirectReply completionReply(objectId, actionId, returnCode);
|
|
|
|
result = CommandingServiceBase::EXECUTION_COMPLETE;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
case ActionMessage::STEP_SUCCESS: {
|
|
|
|
*isStep = true;
|
|
|
|
result = HasReturnvaluesIF::RETURN_OK;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
case ActionMessage::DATA_REPLY: {
|
|
|
|
/* Multiple data replies are possible, so declare data reply as step */
|
|
|
|
*isStep = true;
|
|
|
|
result = handleDataReply(reply, objectId, actionId);
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
case ActionMessage::STEP_FAILED:
|
|
|
|
*isStep = true;
|
|
|
|
/* No break, falls through */
|
|
|
|
case ActionMessage::COMPLETION_FAILED:
|
|
|
|
result = ActionMessage::getReturnCode(reply);
|
|
|
|
break;
|
|
|
|
default:
|
|
|
|
result = INVALID_REPLY;
|
|
|
|
}
|
|
|
|
return result;
|
2020-08-13 20:53:35 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t Service8FunctionManagement::handleDataReply(const CommandMessage* reply,
|
|
|
|
object_id_t objectId,
|
|
|
|
ActionId_t actionId) {
|
|
|
|
store_address_t storeId = ActionMessage::getStoreId(reply);
|
|
|
|
size_t size = 0;
|
|
|
|
const uint8_t* buffer = nullptr;
|
2022-07-20 22:21:15 +02:00
|
|
|
ReturnValue_t result = ipcStore->getData(storeId, &buffer, &size);
|
2022-02-02 10:29:30 +01:00
|
|
|
if (result != RETURN_OK) {
|
2021-01-03 14:16:52 +01:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::error << "Service 8: Could not retrieve data for data reply" << std::endl;
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2022-02-02 10:29:30 +01:00
|
|
|
return result;
|
|
|
|
}
|
|
|
|
DataReply dataReply(objectId, actionId, buffer, size);
|
2022-07-26 13:59:09 +02:00
|
|
|
result = sendTmPacket(static_cast<uint8_t>(Subservice::REPLY_DIRECT_COMMANDING_DATA), dataReply);
|
2020-08-13 20:53:35 +02:00
|
|
|
|
2022-07-20 22:21:15 +02:00
|
|
|
auto deletionResult = ipcStore->deleteData(storeId);
|
2022-02-02 10:29:30 +01:00
|
|
|
if (deletionResult != HasReturnvaluesIF::RETURN_OK) {
|
2021-01-03 14:16:52 +01:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::warning << "Service8FunctionManagement::handleReply: Deletion"
|
|
|
|
<< " of data in pool failed." << std::endl;
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
|
|
|
return result;
|
2020-08-13 20:53:35 +02:00
|
|
|
}
|