continue update
This commit is contained in:
391
src/fsfw/housekeeping/PeriodicHkHelper.cpp
Normal file
391
src/fsfw/housekeeping/PeriodicHkHelper.cpp
Normal file
@ -0,0 +1,391 @@
|
||||
#include "PeriodicHkHelper.h"
|
||||
|
||||
#include <cmath>
|
||||
#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;
|
||||
}
|
||||
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::getSid(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::getSid(message));
|
||||
}
|
||||
|
||||
default:
|
||||
return CommandMessageIF::UNKNOWN_COMMAND;
|
||||
}
|
||||
|
||||
CommandMessage reply;
|
||||
if (result != returnvalue::OK) {
|
||||
if (result == WRONG_HK_PACKET_TYPE) {
|
||||
printWarningOrError(sif::OutputTypes::OUT_WARNING, "handleHousekeepingMessage",
|
||||
WRONG_HK_PACKET_TYPE);
|
||||
}
|
||||
HousekeepingMessage::setHkRequestFailureReply(&reply, sid, result);
|
||||
} else {
|
||||
HousekeepingMessage::setHkRequestSuccessReply(&reply, sid);
|
||||
}
|
||||
hkQueue->sendMessage(hkDestinationId, &reply);
|
||||
return result;
|
||||
}
|
||||
|
||||
PeriodicHkGenerationIF* PeriodicHelper::getOwner() const { return owner; }
|
||||
|
||||
ReturnValue_t PeriodicHelper::generateHousekeepingPacket(const dp::sid_t sid,
|
||||
MessageQueueId_t destination) {
|
||||
store_address_t storeId;
|
||||
auto optSetSpec = getSetSpecification(sid);
|
||||
if (!optSetSpec.has_value()) {
|
||||
return DATASET_NOT_FOUND;
|
||||
}
|
||||
auto& setSpec = optSetSpec.value().get();
|
||||
uint8_t* dataPtr = nullptr;
|
||||
const size_t maxSize = setSpec.size;
|
||||
ReturnValue_t result = ipcStore->getFreeElement(&storeId, maxSize, &dataPtr);
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
result = owner->serializeHkDataset(sid, dataPtr, maxSize);
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
HousekeepingPacketDownlink hkPacket(sid, dataPtr, maxSize);
|
||||
|
||||
size_t serializedSize = 0;
|
||||
result = hkPacket.serialize(&dataPtr, &serializedSize, maxSize, SerializeIF::Endianness::NETWORK);
|
||||
if (result != returnvalue::OK or serializedSize == 0) {
|
||||
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) {
|
||||
if (not setSpec.periodicCollectionEnabled) {
|
||||
return;
|
||||
}
|
||||
|
||||
// timeval now{};
|
||||
Clock::getClockMonotonic(&now);
|
||||
|
||||
sid_t sid = setSpec.dataId.sid;
|
||||
|
||||
timeval diff = now - setSpec.lastGenerated;
|
||||
dur_millis_t diffMillis = diff.tv_sec * 1000 + diff.tv_usec / 1000;
|
||||
if (diffMillis > setSpec.collectionFrequency) {
|
||||
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(dp::sid_t sid, bool enable) {
|
||||
auto optSetSpec = getSetSpecification(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::getSetSpecification(
|
||||
sid_t structureId) {
|
||||
for (auto& receiver : setList) {
|
||||
if (receiver.dataId.sid == structureId) {
|
||||
return receiver;
|
||||
}
|
||||
}
|
||||
return std::nullopt;
|
||||
}
|
||||
|
||||
ReturnValue_t PeriodicHelper::setCollectionInterval(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(sid_t sid) {
|
||||
// Get and check dataset first.
|
||||
auto optSetSpec = getSetSpecification(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.size, &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 sid_t structureId,
|
||||
const std::optional<dur_millis_t> frequencyMs) {
|
||||
// Get and check dataset first.
|
||||
const auto optSetSpec = getSetSpecification(structureId);
|
||||
if (!optSetSpec.has_value()) {
|
||||
printWarningOrError(sif::OutputTypes::OUT_WARNING, "performPeriodicHkGeneration",
|
||||
DATASET_NOT_FOUND);
|
||||
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 sid_t structureId) {
|
||||
// Get and check dataset first.
|
||||
const auto optSetSpec = getSetSpecification(structureId);
|
||||
if (!optSetSpec.has_value()) {
|
||||
printWarningOrError(sif::OutputTypes::OUT_WARNING, "performPeriodicHkGeneration",
|
||||
DATASET_NOT_FOUND);
|
||||
return DATASET_NOT_FOUND;
|
||||
}
|
||||
auto& setSpec = optSetSpec.value().get();
|
||||
setSpec.periodicCollectionEnabled = false;
|
||||
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 == POOLOBJECT_NOT_FOUND) {
|
||||
errorPrint = "Pool Object not found";
|
||||
} else if (error == WRONG_HK_PACKET_TYPE) {
|
||||
errorPrint = "Wrong Packet Type";
|
||||
} 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 == localpool::POOL_ENTRY_TYPE_CONFLICT) {
|
||||
errorPrint = "Pool entry type conflict";
|
||||
} else if (error == localpool::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);
|
||||
}
|
||||
Reference in New Issue
Block a user