2021-07-13 20:58:45 +02:00
|
|
|
#include "fsfw/pus/Service20ParameterManagement.h"
|
2021-01-30 20:29:23 +01:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
#include "fsfw/objectmanager/ObjectManager.h"
|
2021-07-13 20:58:45 +02:00
|
|
|
#include "fsfw/parameters/HasParametersIF.h"
|
|
|
|
#include "fsfw/parameters/ParameterMessage.h"
|
|
|
|
#include "fsfw/parameters/ReceivesParameterMessagesIF.h"
|
2022-02-02 10:29:30 +01:00
|
|
|
#include "fsfw/pus/servicepackets/Service20Packets.h"
|
|
|
|
#include "fsfw/serviceinterface/ServiceInterface.h"
|
2021-01-30 20:29:23 +01:00
|
|
|
|
|
|
|
Service20ParameterManagement::Service20ParameterManagement(object_id_t objectId, uint16_t apid,
|
2022-02-02 10:29:30 +01:00
|
|
|
uint8_t serviceId,
|
|
|
|
uint8_t numberOfParallelCommands,
|
|
|
|
uint16_t commandTimeoutSeconds)
|
|
|
|
: CommandingServiceBase(objectId, apid, serviceId, numberOfParallelCommands,
|
|
|
|
commandTimeoutSeconds) {}
|
2021-01-30 20:29:23 +01:00
|
|
|
|
|
|
|
Service20ParameterManagement::~Service20ParameterManagement() {}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t Service20ParameterManagement::isValidSubservice(uint8_t subservice) {
|
|
|
|
switch (static_cast<Subservice>(subservice)) {
|
2021-01-30 20:29:23 +01:00
|
|
|
case Subservice::PARAMETER_LOAD:
|
|
|
|
case Subservice::PARAMETER_DUMP:
|
2022-02-02 10:29:30 +01:00
|
|
|
return HasReturnvaluesIF::RETURN_OK;
|
2021-01-30 20:29:23 +01:00
|
|
|
default:
|
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::error << "Invalid Subservice for Service 20" << std::endl;
|
2021-01-30 20:29:23 +01:00
|
|
|
#else
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::printError("Invalid Subservice for Service 20\n");
|
2021-01-30 20:29:23 +01:00
|
|
|
#endif
|
2022-02-02 10:29:30 +01:00
|
|
|
return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
|
|
|
|
}
|
2021-01-30 20:29:23 +01:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t Service20ParameterManagement::getMessageQueueAndObject(uint8_t subservice,
|
|
|
|
const uint8_t* tcData,
|
|
|
|
size_t tcDataLen,
|
|
|
|
MessageQueueId_t* id,
|
|
|
|
object_id_t* objectId) {
|
|
|
|
ReturnValue_t result = checkAndAcquireTargetID(objectId, tcData, tcDataLen);
|
|
|
|
if (result != RETURN_OK) {
|
|
|
|
return result;
|
|
|
|
}
|
|
|
|
return checkInterfaceAndAcquireMessageQueue(id, objectId);
|
2021-01-30 20:29:23 +01:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t Service20ParameterManagement::checkAndAcquireTargetID(object_id_t* objectIdToSet,
|
|
|
|
const uint8_t* tcData,
|
|
|
|
size_t tcDataLen) {
|
|
|
|
if (SerializeAdapter::deSerialize(objectIdToSet, &tcData, &tcDataLen,
|
|
|
|
SerializeIF::Endianness::BIG) != HasReturnvaluesIF::RETURN_OK) {
|
2021-01-30 20:29:23 +01:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::error << "Service20ParameterManagement::checkAndAcquireTargetID: "
|
|
|
|
<< "Invalid data." << std::endl;
|
2021-01-30 20:29:23 +01:00
|
|
|
#else
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::printError(
|
|
|
|
"Service20ParameterManagement::"
|
|
|
|
"checkAndAcquireTargetID: Invalid data.\n");
|
2021-01-30 20:29:23 +01:00
|
|
|
#endif
|
2022-02-02 10:29:30 +01:00
|
|
|
return CommandingServiceBase::INVALID_TC;
|
|
|
|
}
|
|
|
|
return HasReturnvaluesIF::RETURN_OK;
|
2021-01-30 20:29:23 +01:00
|
|
|
}
|
|
|
|
|
|
|
|
ReturnValue_t Service20ParameterManagement::checkInterfaceAndAcquireMessageQueue(
|
2022-02-02 10:29:30 +01:00
|
|
|
MessageQueueId_t* messageQueueToSet, object_id_t* objectId) {
|
|
|
|
// check ReceivesParameterMessagesIF property of target
|
|
|
|
ReceivesParameterMessagesIF* possibleTarget =
|
|
|
|
ObjectManager::instance()->get<ReceivesParameterMessagesIF>(*objectId);
|
|
|
|
if (possibleTarget == nullptr) {
|
2021-01-30 20:29:23 +01:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::error << "Service20ParameterManagement::checkInterfaceAndAcquire"
|
|
|
|
<< "MessageQueue: Can't access object" << std::endl;
|
|
|
|
sif::error << "Object ID: " << std::hex << objectId << std::dec << std::endl;
|
|
|
|
sif::error << "Make sure it implements ReceivesParameterMessagesIF!" << std::endl;
|
2021-01-30 20:29:23 +01:00
|
|
|
#else
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::printError(
|
|
|
|
"Service20ParameterManagement::checkInterfaceAndAcquire"
|
|
|
|
"MessageQueue: Can't access object\n");
|
|
|
|
sif::printError("Object ID: 0x%08x\n", *objectId);
|
|
|
|
sif::printError("Make sure it implements ReceivesParameterMessagesIF!\n");
|
2021-01-30 20:29:23 +01:00
|
|
|
#endif
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
return CommandingServiceBase::INVALID_OBJECT;
|
|
|
|
}
|
|
|
|
*messageQueueToSet = possibleTarget->getCommandQueue();
|
|
|
|
return HasReturnvaluesIF::RETURN_OK;
|
2021-01-30 20:29:23 +01:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t Service20ParameterManagement::prepareCommand(CommandMessage* message,
|
|
|
|
uint8_t subservice,
|
|
|
|
const uint8_t* tcData, size_t tcDataLen,
|
|
|
|
uint32_t* state, object_id_t objectId) {
|
|
|
|
switch (static_cast<Subservice>(subservice)) {
|
2021-01-30 20:29:23 +01:00
|
|
|
case Subservice::PARAMETER_DUMP: {
|
2022-02-02 10:29:30 +01:00
|
|
|
return prepareDumpCommand(message, tcData, tcDataLen);
|
|
|
|
} break;
|
2021-01-30 20:29:23 +01:00
|
|
|
case Subservice::PARAMETER_LOAD: {
|
2022-02-02 10:29:30 +01:00
|
|
|
return prepareLoadCommand(message, tcData, tcDataLen);
|
|
|
|
} break;
|
2021-01-30 20:29:23 +01:00
|
|
|
default:
|
2022-02-02 10:29:30 +01:00
|
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
|
|
}
|
2021-01-30 20:29:23 +01:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t Service20ParameterManagement::prepareDumpCommand(CommandMessage* message,
|
|
|
|
const uint8_t* tcData,
|
|
|
|
size_t tcDataLen) {
|
|
|
|
/* the first part is the objectId, but we have extracted that earlier
|
|
|
|
and only need the parameterId */
|
|
|
|
tcData += sizeof(object_id_t);
|
|
|
|
tcDataLen -= sizeof(object_id_t);
|
|
|
|
ParameterId_t parameterId;
|
|
|
|
if (SerializeAdapter::deSerialize(¶meterId, &tcData, &tcDataLen,
|
|
|
|
SerializeIF::Endianness::BIG) != HasReturnvaluesIF::RETURN_OK) {
|
|
|
|
return CommandingServiceBase::INVALID_TC;
|
|
|
|
}
|
|
|
|
/* The length should have been decremented to 0 by this point */
|
|
|
|
if (tcDataLen != 0) {
|
|
|
|
return CommandingServiceBase::INVALID_TC;
|
|
|
|
}
|
|
|
|
|
|
|
|
ParameterMessage::setParameterDumpCommand(message, parameterId);
|
|
|
|
return HasReturnvaluesIF::RETURN_OK;
|
2021-01-30 20:29:23 +01:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t Service20ParameterManagement::prepareLoadCommand(CommandMessage* message,
|
|
|
|
const uint8_t* tcData,
|
|
|
|
size_t tcDataLen) {
|
|
|
|
if (tcDataLen < sizeof(object_id_t) + sizeof(ParameterId_t) + sizeof(uint32_t)) {
|
|
|
|
return CommandingServiceBase::INVALID_TC;
|
|
|
|
}
|
|
|
|
|
|
|
|
uint8_t* storePointer = nullptr;
|
|
|
|
store_address_t storeAddress;
|
|
|
|
size_t parameterDataLen =
|
|
|
|
tcDataLen - sizeof(object_id_t) - sizeof(ParameterId_t) - sizeof(uint32_t);
|
|
|
|
if (parameterDataLen == 0) {
|
|
|
|
return CommandingServiceBase::INVALID_TC;
|
|
|
|
}
|
2022-07-20 22:21:15 +02:00
|
|
|
ReturnValue_t result = ipcStore->getFreeElement(&storeAddress, parameterDataLen, &storePointer);
|
2022-02-02 10:29:30 +01:00
|
|
|
if (result != HasReturnvaluesIF::RETURN_OK) {
|
|
|
|
return result;
|
|
|
|
}
|
|
|
|
|
|
|
|
/* Following format is expected: The first 4 bytes in the TC data are the 4 byte
|
|
|
|
parameter ID (ParameterId_t). The second 4 bytes are the parameter information field,
|
|
|
|
containing the following 1 byte fields:
|
|
|
|
1. ECSS PTC field
|
|
|
|
2. ECSS PFC field
|
|
|
|
3. Number of rows
|
|
|
|
4. Number of columns */
|
|
|
|
ParameterLoadCommand command(storePointer, parameterDataLen);
|
|
|
|
result = command.deSerialize(&tcData, &tcDataLen, SerializeIF::Endianness::BIG);
|
|
|
|
if (result != HasReturnvaluesIF::RETURN_OK) {
|
|
|
|
return result;
|
|
|
|
}
|
|
|
|
|
|
|
|
ParameterMessage::setParameterLoadCommand(message, command.getParameterId(), storeAddress,
|
|
|
|
command.getPtc(), command.getPfc(), command.getRows(),
|
|
|
|
command.getColumns());
|
|
|
|
return HasReturnvaluesIF::RETURN_OK;
|
2021-01-30 20:29:23 +01:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t Service20ParameterManagement::handleReply(const CommandMessage* reply,
|
|
|
|
Command_t previousCommand, uint32_t* state,
|
|
|
|
CommandMessage* optionalNextCommand,
|
|
|
|
object_id_t objectId, bool* isStep) {
|
|
|
|
Command_t replyId = reply->getCommand();
|
2021-01-30 20:29:23 +01:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
switch (replyId) {
|
2021-01-30 20:29:23 +01:00
|
|
|
case ParameterMessage::REPLY_PARAMETER_DUMP: {
|
2022-07-20 22:21:15 +02:00
|
|
|
ConstAccessorPair parameterData = ipcStore->getData(ParameterMessage::getStoreId(reply));
|
2022-02-02 10:29:30 +01:00
|
|
|
if (parameterData.first != HasReturnvaluesIF::RETURN_OK) {
|
|
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
|
|
}
|
|
|
|
|
|
|
|
ParameterId_t parameterId = ParameterMessage::getParameterId(reply);
|
|
|
|
ParameterDumpReply parameterReply(objectId, parameterId, parameterData.second.data(),
|
|
|
|
parameterData.second.size());
|
2022-07-25 11:26:45 +02:00
|
|
|
sendTmPacket(static_cast<uint8_t>(Subservice::PARAMETER_DUMP_REPLY), parameterReply);
|
2022-02-02 10:29:30 +01:00
|
|
|
return HasReturnvaluesIF::RETURN_OK;
|
2021-01-30 20:29:23 +01:00
|
|
|
}
|
|
|
|
default:
|
2022-02-02 10:29:30 +01:00
|
|
|
return CommandingServiceBase::INVALID_REPLY;
|
|
|
|
}
|
2021-01-30 20:29:23 +01:00
|
|
|
}
|