fsfw/tasks/FixedSlotSequence.cpp

175 lines
5.3 KiB
C++
Raw Normal View History

2020-09-04 14:11:59 +02:00
#include "FixedSlotSequence.h"
2020-09-04 13:52:54 +02:00
#include "../serviceinterface/ServiceInterfaceStream.h"
#include <cstdlib>
FixedSlotSequence::FixedSlotSequence(uint32_t setLengthMs) :
lengthMs(setLengthMs) {
current = slotList.begin();
}
FixedSlotSequence::~FixedSlotSequence() {
// Call the destructor on each list entry.
slotList.clear();
}
void FixedSlotSequence::executeAndAdvance() {
current->executableObject->performOperation(current->opcode);
// if (returnValue != RETURN_OK) {
// this->sendErrorMessage( returnValue );
// }
//Increment the polling Sequence iterator
this->current++;
//Set it to the beginning, if the list's end is reached.
if (this->current == this->slotList.end()) {
this->current = this->slotList.begin();
}
}
uint32_t FixedSlotSequence::getIntervalToNextSlotMs() {
uint32_t oldTime;
SlotListIter slotListIter = current;
// Get the pollingTimeMs of the current slot object.
oldTime = slotListIter->pollingTimeMs;
// Advance to the next object.
slotListIter++;
// Find the next interval which is not 0.
while (slotListIter != slotList.end()) {
if (oldTime != slotListIter->pollingTimeMs) {
return slotListIter->pollingTimeMs - oldTime;
} else {
slotListIter++;
}
}
// If the list end is reached (this is definitely an interval != 0),
// the interval is calculated by subtracting the remaining time of the PST
// and adding the start time of the first handler in the list.
slotListIter = slotList.begin();
return lengthMs - oldTime + slotListIter->pollingTimeMs;
}
uint32_t FixedSlotSequence::getIntervalToPreviousSlotMs() {
uint32_t currentTime;
SlotListIter slotListIter = current;
// Get the pollingTimeMs of the current slot object.
currentTime = slotListIter->pollingTimeMs;
//if it is the first slot, calculate difference to last slot
if (slotListIter == slotList.begin()){
return lengthMs - (--slotList.end())->pollingTimeMs + currentTime;
}
// get previous slot
slotListIter--;
return currentTime - slotListIter->pollingTimeMs;
}
bool FixedSlotSequence::slotFollowsImmediately() {
uint32_t currentTime = current->pollingTimeMs;
SlotListIter fixedSequenceIter = this->current;
// Get the pollingTimeMs of the current slot object.
if (fixedSequenceIter == slotList.begin())
return false;
fixedSequenceIter--;
if (fixedSequenceIter->pollingTimeMs == currentTime) {
return true;
} else {
return false;
}
}
uint32_t FixedSlotSequence::getLengthMs() const {
return this->lengthMs;
}
void FixedSlotSequence::addSlot(object_id_t componentId, uint32_t slotTimeMs,
int8_t executionStep, ExecutableObjectIF* executableObject,
PeriodicTaskIF* executingTask) {
this->slotList.insert(FixedSequenceSlot(componentId, slotTimeMs,
executionStep, executableObject, executingTask));
this->current = slotList.begin();
}
ReturnValue_t FixedSlotSequence::checkSequence() const {
if(slotList.empty()) {
2021-01-03 14:16:52 +01:00
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "FixedSlotSequence::checkSequence:"
<< " Slot list is empty!" << std::endl;
#endif
2020-09-04 13:52:54 +02:00
return HasReturnvaluesIF::RETURN_FAILED;
}
if(customCheckFunction != nullptr) {
ReturnValue_t result = customCheckFunction(slotList);
if(result != HasReturnvaluesIF::RETURN_OK) {
// Continue for now but print error output.
2021-01-03 14:16:52 +01:00
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "FixedSlotSequence::checkSequence:"
<< " Custom check failed!" << std::endl;
#endif
}
}
uint32_t errorCount = 0;
2020-09-04 13:52:54 +02:00
uint32_t time = 0;
for(const auto& slot: slotList) {
if (slot.executableObject == nullptr) {
errorCount++;
2020-09-04 13:52:54 +02:00
}
else if (slot.pollingTimeMs < time) {
2021-01-03 14:16:52 +01:00
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "FixedSlotSequence::checkSequence: Time: "
2020-09-04 13:52:54 +02:00
<< slot.pollingTimeMs << " is smaller than previous with "
<< time << std::endl;
#endif
errorCount++;
2020-09-04 13:52:54 +02:00
}
else {
// All ok, print slot.
2021-01-03 14:16:52 +01:00
#if FSFW_CPP_OSTREAM_ENABLED == 1
2020-09-04 13:52:54 +02:00
//sif::info << "Current slot polling time: " << std::endl;
//sif::info << std::dec << slotIt->pollingTimeMs << std::endl;
#endif
2020-09-04 13:52:54 +02:00
}
time = slot.pollingTimeMs;
}
2021-01-03 14:16:52 +01:00
#if FSFW_CPP_OSTREAM_ENABLED == 1
2020-09-04 13:52:54 +02:00
//sif::info << "Number of elements in slot list: "
// << slotList.size() << std::endl;
#endif
if (errorCount > 0) {
2020-09-04 13:52:54 +02:00
return HasReturnvaluesIF::RETURN_FAILED;
}
return HasReturnvaluesIF::RETURN_OK;
}
ReturnValue_t FixedSlotSequence::intializeSequenceAfterTaskCreation() const {
std::set<ExecutableObjectIF*> uniqueObjects;
uint32_t count = 0;
for(const auto& slot: slotList) {
// Ensure that each unique object is initialized once.
if(uniqueObjects.find(slot.executableObject) == uniqueObjects.end()) {
ReturnValue_t result =
slot.executableObject->initializeAfterTaskCreation();
if(result != HasReturnvaluesIF::RETURN_OK) {
count++;
}
uniqueObjects.emplace(slot.executableObject);
}
}
if (count > 0) {
2021-01-03 14:16:52 +01:00
#if FSFW_CPP_OSTREAM_ENABLED == 1
2020-09-04 13:52:54 +02:00
sif::error << "FixedSlotSequence::intializeSequenceAfterTaskCreation:"
"Counted " << count << " failed initializations!" << std::endl;
#endif
2020-09-04 13:52:54 +02:00
return HasReturnvaluesIF::RETURN_FAILED;
}
return HasReturnvaluesIF::RETURN_OK;
}
void FixedSlotSequence::addCustomCheck(ReturnValue_t
(*customCheckFunction)(const SlotList&)) {
this->customCheckFunction = customCheckFunction;
}