408 lines
14 KiB
C++
408 lines
14 KiB
C++
#include "PeriodicHkHelper.h"
|
|
|
|
#include <cmath>
|
|
#include <functional>
|
|
#include <set>
|
|
|
|
#include "fsfw/housekeeping/AcceptsHkPacketsIF.h"
|
|
#include "fsfw/housekeeping/HousekeepingSetPacket.h"
|
|
#include "fsfw/housekeeping/HousekeepingSnapshot.h"
|
|
#include "fsfw/ipc/QueueFactory.h"
|
|
#include "fsfw/objectmanager/ObjectManager.h"
|
|
#include "fsfw/timemanager/CCSDSTime.h"
|
|
|
|
using namespace hk;
|
|
|
|
PeriodicHelper::PeriodicHelper(GeneratesPeriodicHkIF* owner, MessageQueueIF* queueToUse,
|
|
MessageQueueId_t hkDestQueue)
|
|
: hkDestinationId(hkDestQueue) {
|
|
if (owner == nullptr) {
|
|
printWarningOrError(sif::OutputTypes::OUT_WARNING, "LocalDataPoolManager", returnvalue::FAILED,
|
|
"Invalid supplied owner");
|
|
return;
|
|
}
|
|
this->owner = owner;
|
|
hkQueue = queueToUse;
|
|
}
|
|
|
|
ReturnValue_t PeriodicHelper::initialize(MessageQueueIF* queueToUse) {
|
|
if (queueToUse != nullptr) {
|
|
hkQueue = queueToUse;
|
|
}
|
|
if (hkQueue == nullptr) {
|
|
// Error, all destinations invalid
|
|
printWarningOrError(sif::OutputTypes::OUT_ERROR, "initialize", QUEUE_OR_DESTINATION_INVALID);
|
|
}
|
|
|
|
ipcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
|
|
if (ipcStore == nullptr) {
|
|
// Error, all destinations invalid
|
|
printWarningOrError(sif::OutputTypes::OUT_ERROR, "initialize", returnvalue::FAILED,
|
|
"Could not set IPC store.");
|
|
return returnvalue::FAILED;
|
|
}
|
|
|
|
if (hkDestinationId == MessageQueueIF::NO_QUEUE) {
|
|
if (const auto* hkPacketReceiver =
|
|
ObjectManager::instance()->get<AcceptsHkPacketsIF>(objects::PUS_SERVICE_3_HOUSEKEEPING);
|
|
hkPacketReceiver != nullptr) {
|
|
hkDestinationId = hkPacketReceiver->getHkQueue();
|
|
} else {
|
|
printWarningOrError(sif::OutputTypes::OUT_ERROR, "initialize", QUEUE_OR_DESTINATION_INVALID);
|
|
return QUEUE_OR_DESTINATION_INVALID;
|
|
}
|
|
}
|
|
|
|
owner->specifyHkDatasets(setList);
|
|
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
ReturnValue_t PeriodicHelper::performHkOperation() {
|
|
timeval now{};
|
|
Clock::getClockMonotonic(&now);
|
|
for (auto& setSpec : setList) {
|
|
switch (setSpec.reportingType) {
|
|
case (ReportingType::PERIODIC): {
|
|
if (setSpec.dataType == DataType::LOCAL_POOL_VARIABLE) {
|
|
// Periodic packets shall only be generated from datasets
|
|
continue;
|
|
}
|
|
if (not setSpec.periodicCollectionEnabled) {
|
|
continue;
|
|
}
|
|
performPeriodicHkGeneration(setSpec, now);
|
|
break;
|
|
}
|
|
default:
|
|
// This should never happen.
|
|
return returnvalue::FAILED;
|
|
}
|
|
}
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
ReturnValue_t PeriodicHelper::handleHousekeepingMessage(CommandMessage* message) {
|
|
Command_t command = message->getCommand();
|
|
dp::sid_t sid = HousekeepingMessage::getStructureId(message);
|
|
ReturnValue_t result = returnvalue::OK;
|
|
switch (command) {
|
|
// Houskeeping interface handling.
|
|
case (HousekeepingMessage::ENABLE_PERIODIC_HK_REPORT_GENERATION): {
|
|
result = togglePeriodicGeneration(sid, true);
|
|
break;
|
|
}
|
|
|
|
case (HousekeepingMessage::DISABLE_PERIODIC_HK_REPORT_GENERATION): {
|
|
result = togglePeriodicGeneration(sid, false);
|
|
break;
|
|
}
|
|
|
|
case (HousekeepingMessage::REPORT_HK_REPORT_STRUCTURES): {
|
|
result = generateSetStructurePacket(sid);
|
|
if (result == returnvalue::OK) {
|
|
return result;
|
|
}
|
|
break;
|
|
}
|
|
case (HousekeepingMessage::MODIFY_PARAMETER_REPORT_COLLECTION_INTERVAL): {
|
|
dur_millis_t newCollIntvl = 0;
|
|
HousekeepingMessage::getCollectionIntervalModificationCommand(message, newCollIntvl);
|
|
result = setCollectionInterval(sid, newCollIntvl);
|
|
break;
|
|
}
|
|
|
|
case (HousekeepingMessage::GENERATE_ONE_PARAMETER_REPORT): {
|
|
return generateHousekeepingPacket(HousekeepingMessage::getStructureId(message));
|
|
}
|
|
|
|
default:
|
|
return CommandMessageIF::UNKNOWN_COMMAND;
|
|
}
|
|
|
|
CommandMessage reply;
|
|
if (result != returnvalue::OK) {
|
|
if (result == INVALID_HK_REQUEST) {
|
|
printWarningOrError(sif::OutputTypes::OUT_WARNING, "handleHousekeepingMessage",
|
|
INVALID_HK_REQUEST);
|
|
}
|
|
HousekeepingMessage::setHkRequestFailureReply(&reply, sid, result);
|
|
} else {
|
|
HousekeepingMessage::setHkRequestSuccessReply(&reply, sid);
|
|
}
|
|
hkQueue->sendMessage(hkDestinationId, &reply);
|
|
return result;
|
|
}
|
|
|
|
GeneratesPeriodicHkIF* PeriodicHelper::getOwner() const { return owner; }
|
|
|
|
ReturnValue_t PeriodicHelper::generateHousekeepingPacket(const dp::sid_t sid,
|
|
MessageQueueId_t destination) {
|
|
store_address_t storeId;
|
|
const auto optSetSpec = getMutSetSpecification(sid);
|
|
if (!optSetSpec.has_value()) {
|
|
return DATASET_NOT_FOUND;
|
|
}
|
|
const auto& setSpec = optSetSpec.value().get();
|
|
uint8_t* dataPtr = nullptr;
|
|
const size_t maxSize = setSpec.serializedSize + dp::structure_id_t::SIZE;
|
|
ReturnValue_t result = ipcStore->getFreeElement(&storeId, maxSize, &dataPtr);
|
|
if (result != returnvalue::OK) {
|
|
return result;
|
|
}
|
|
size_t serSize = 0;
|
|
result = SerializeAdapter::serialize(&sid.objectId, &dataPtr, &serSize, maxSize,
|
|
SerializeIF::Endianness::NETWORK);
|
|
if (result != returnvalue::OK) {
|
|
return result;
|
|
}
|
|
result = SerializeAdapter::serialize(&sid.ownerSetId, &dataPtr, &serSize, maxSize,
|
|
SerializeIF::Endianness::NETWORK);
|
|
if (result != returnvalue::OK) {
|
|
return result;
|
|
}
|
|
result = owner->serializeHkDataset(sid, dataPtr, maxSize - 8);
|
|
if (result != returnvalue::OK) {
|
|
return result;
|
|
}
|
|
|
|
// Now we set a HK message and send it the HK packet destination.
|
|
CommandMessage hkMessage;
|
|
HousekeepingMessage::setHkReportReply(&hkMessage, sid, storeId);
|
|
|
|
if (hkQueue == nullptr) {
|
|
// Error, no queue available to send packet with.
|
|
printWarningOrError(sif::OutputTypes::OUT_WARNING, "generateHousekeepingPacket",
|
|
QUEUE_OR_DESTINATION_INVALID);
|
|
return QUEUE_OR_DESTINATION_INVALID;
|
|
}
|
|
if (destination == MessageQueueIF::NO_QUEUE) {
|
|
if (hkDestinationId == MessageQueueIF::NO_QUEUE) {
|
|
// Error, all destinations invalid
|
|
printWarningOrError(sif::OutputTypes::OUT_WARNING, "generateHousekeepingPacket",
|
|
QUEUE_OR_DESTINATION_INVALID);
|
|
return QUEUE_OR_DESTINATION_INVALID;
|
|
}
|
|
destination = hkDestinationId;
|
|
}
|
|
|
|
return hkQueue->sendMessage(destination, &hkMessage);
|
|
}
|
|
|
|
void PeriodicHelper::performPeriodicHkGeneration(SetSpecification& setSpec, timeval& now) {
|
|
const dp::sid_t sid = setSpec.dataId.sid;
|
|
|
|
auto [tv_sec, tv_usec] = now - setSpec.lastGenerated;
|
|
if (const dur_millis_t diffMillis = tv_sec * 1000 + tv_usec / 1000;
|
|
diffMillis >= setSpec.collectionFrequency) {
|
|
const ReturnValue_t result = generateHousekeepingPacket(sid);
|
|
setSpec.lastGenerated = now;
|
|
if (result != returnvalue::OK) {
|
|
// Configuration error
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::warning << "LocalDataPoolManager::performPeriodicHkOperation: HK generation failed."
|
|
<< std::endl;
|
|
#else
|
|
sif::printWarning(
|
|
"LocalDataPoolManager::performPeriodicHkOperation: HK generation failed.\n");
|
|
#endif
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
|
|
ReturnValue_t PeriodicHelper::togglePeriodicGeneration(const dp::sid_t sid, const bool enable) {
|
|
const auto optSetSpec = getMutSetSpecification(sid);
|
|
if (!optSetSpec.has_value()) {
|
|
printWarningOrError(sif::OutputTypes::OUT_WARNING, "togglePeriodicGeneration",
|
|
DATASET_NOT_FOUND);
|
|
return DATASET_NOT_FOUND;
|
|
}
|
|
optSetSpec.value().get().periodicCollectionEnabled = enable;
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
std::optional<std::reference_wrapper<SetSpecification>> PeriodicHelper::getMutSetSpecification(
|
|
dp::sid_t structureId) {
|
|
for (auto& receiver : setList) {
|
|
if (receiver.dataId.sid == structureId) {
|
|
return receiver;
|
|
}
|
|
}
|
|
return std::nullopt;
|
|
}
|
|
|
|
std::optional<std::reference_wrapper<const SetSpecification>> PeriodicHelper::getSetSpecification(
|
|
dp::sid_t structureId) const {
|
|
for (const auto& receiver : setList) {
|
|
if (receiver.dataId.sid == structureId) {
|
|
return receiver;
|
|
}
|
|
}
|
|
return std::nullopt;
|
|
}
|
|
|
|
ReturnValue_t PeriodicHelper::setCollectionInterval(dp::sid_t sid,
|
|
dur_millis_t newCollectionIntervalMs) {
|
|
bool wasUpdated = false;
|
|
for (auto& receiver : setList) {
|
|
if (receiver.dataId.sid == sid) {
|
|
receiver.collectionFrequency = newCollectionIntervalMs;
|
|
wasUpdated = true;
|
|
}
|
|
}
|
|
if (!wasUpdated) {
|
|
printWarningOrError(sif::OutputTypes::OUT_WARNING, "setCollectionInterval", DATASET_NOT_FOUND);
|
|
return DATASET_NOT_FOUND;
|
|
}
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
ReturnValue_t PeriodicHelper::generateSetStructurePacket(dp::structure_id_t sid) {
|
|
// Get and check dataset first.
|
|
auto optSetSpec = getMutSetSpecification(sid);
|
|
if (!optSetSpec.has_value()) {
|
|
printWarningOrError(sif::OutputTypes::OUT_WARNING, "performPeriodicHkGeneration",
|
|
DATASET_NOT_FOUND);
|
|
return DATASET_NOT_FOUND;
|
|
}
|
|
auto& setSpec = optSetSpec.value().get();
|
|
|
|
uint8_t* storePtr = nullptr;
|
|
store_address_t storeId;
|
|
ReturnValue_t result = ipcStore->getFreeElement(&storeId, setSpec.serializedSize, &storePtr);
|
|
if (result != returnvalue::OK) {
|
|
printWarningOrError(sif::OutputTypes::OUT_ERROR, "generateSetStructurePacket",
|
|
returnvalue::FAILED, "Could not get free element from IPC store.");
|
|
return result;
|
|
}
|
|
|
|
dur_millis_t collectionInterval = 0;
|
|
|
|
HousekeepingSetPacket setPacket(sid, setSpec.periodicCollectionEnabled, collectionInterval);
|
|
if (result != returnvalue::OK) {
|
|
return result;
|
|
}
|
|
size_t expectedSize = setPacket.getSerializedSize();
|
|
|
|
// Serialize set packet into store.
|
|
size_t size = 0;
|
|
result = setPacket.serialize(&storePtr, &size, expectedSize, SerializeIF::Endianness::NETWORK);
|
|
if (result != returnvalue::OK) {
|
|
ipcStore->deleteData(storeId);
|
|
return result;
|
|
}
|
|
if (expectedSize != size) {
|
|
printWarningOrError(sif::OutputTypes::OUT_WARNING, "generateSetStructurePacket",
|
|
returnvalue::FAILED, "Expected size is not equal to serialized size");
|
|
}
|
|
|
|
// Send structure reporting reply.
|
|
CommandMessage reply;
|
|
HousekeepingMessage::setHkStuctureReportReply(&reply, sid, storeId);
|
|
|
|
result = hkQueue->reply(&reply);
|
|
if (result != returnvalue::OK) {
|
|
ipcStore->deleteData(storeId);
|
|
}
|
|
return result;
|
|
}
|
|
|
|
ReturnValue_t PeriodicHelper::enablePeriodicPacket(const dp::structure_id_t structureId,
|
|
const std::optional<dur_millis_t> frequencyMs) {
|
|
// Get and check dataset first.
|
|
const auto optSetSpec = getMutSetSpecification(structureId);
|
|
if (!optSetSpec.has_value()) {
|
|
return DATASET_NOT_FOUND;
|
|
}
|
|
auto& setSpec = optSetSpec.value().get();
|
|
setSpec.periodicCollectionEnabled = true;
|
|
if (frequencyMs) {
|
|
setSpec.collectionFrequency = frequencyMs.value();
|
|
}
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
ReturnValue_t PeriodicHelper::disablePeriodicPacket(const dp::structure_id_t structureId) {
|
|
// Get and check dataset first.
|
|
const auto optSetSpec = getMutSetSpecification(structureId);
|
|
if (!optSetSpec.has_value()) {
|
|
return DATASET_NOT_FOUND;
|
|
}
|
|
auto& setSpec = optSetSpec.value().get();
|
|
setSpec.periodicCollectionEnabled = false;
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
ReturnValue_t PeriodicHelper::collectionEnabled(dp::sid_t structureId,
|
|
bool& collectionEnabled) const {
|
|
// Get and check dataset first.
|
|
const auto optSetSpec = getSetSpecification(structureId);
|
|
if (!optSetSpec.has_value()) {
|
|
return DATASET_NOT_FOUND;
|
|
}
|
|
auto& setSpec = optSetSpec.value().get();
|
|
collectionEnabled = setSpec.periodicCollectionEnabled;
|
|
return returnvalue::OK;
|
|
}
|
|
object_id_t PeriodicHelper::getCreatorObjectId() const { return owner->getObjectId(); }
|
|
|
|
void PeriodicHelper::printWarningOrError(sif::OutputTypes outputType, const char* functionName,
|
|
ReturnValue_t error, const char* errorPrint) {
|
|
#if FSFW_VERBOSE_LEVEL >= 1
|
|
if (errorPrint == nullptr) {
|
|
if (error == DATASET_NOT_FOUND) {
|
|
errorPrint = "Dataset not found";
|
|
} else if (error == INVALID_HK_REQUEST) {
|
|
errorPrint = "Invalid HK request";
|
|
} else if (error == returnvalue::FAILED) {
|
|
if (outputType == sif::OutputTypes::OUT_WARNING) {
|
|
errorPrint = "Generic Warning";
|
|
} else {
|
|
errorPrint = "Generic error";
|
|
}
|
|
} else if (error == QUEUE_OR_DESTINATION_INVALID) {
|
|
errorPrint = "Queue or destination not set";
|
|
} else if (error == dp::POOL_ENTRY_TYPE_CONFLICT) {
|
|
errorPrint = "Pool entry type conflict";
|
|
} else if (error == dp::POOL_ENTRY_NOT_FOUND) {
|
|
errorPrint = "Pool entry not found";
|
|
} else {
|
|
errorPrint = "Unknown error";
|
|
}
|
|
}
|
|
object_id_t objectId = 0xffffffff;
|
|
if (owner != nullptr) {
|
|
objectId = owner->getObjectId();
|
|
}
|
|
|
|
if (outputType == sif::OutputTypes::OUT_WARNING) {
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::warning << "LocalDataPoolManager::" << functionName << ": Object ID 0x" << std::setw(8)
|
|
<< std::setfill('0') << std::hex << objectId << " | " << errorPrint << std::dec
|
|
<< std::setfill(' ') << std::endl;
|
|
#else
|
|
sif::printWarning("LocalDataPoolManager::%s: Object ID 0x%08x | %s\n", functionName, objectId,
|
|
errorPrint);
|
|
#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
|
|
} else if (outputType == sif::OutputTypes::OUT_ERROR) {
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "LocalDataPoolManager::" << functionName << ": Object ID 0x" << std::setw(8)
|
|
<< std::setfill('0') << std::hex << objectId << " | " << errorPrint << std::dec
|
|
<< std::setfill(' ') << std::endl;
|
|
#else
|
|
sif::printError("LocalDataPoolManager::%s: Object ID 0x%08x | %s\n", functionName, objectId,
|
|
errorPrint);
|
|
#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
|
|
}
|
|
#endif /* #if FSFW_VERBOSE_LEVEL >= 1 */
|
|
}
|
|
|
|
void PeriodicHelper::setHkDestinationId(const MessageQueueId_t hkDestId) {
|
|
hkDestinationId = hkDestId;
|
|
}
|
|
|
|
void PeriodicHelper::addSetSpecification(const SetSpecification& setSpec) {
|
|
setList.push_back(setSpec);
|
|
}
|