fsfw-example-common/example/test/FsfwExampleTask.cpp

276 lines
8.4 KiB
C++
Raw Normal View History

2021-06-08 13:35:49 +02:00
#include "FsfwExampleTask.h"
2022-05-05 20:55:28 +02:00
#include <fsfw/ipc/CommandMessage.h>
#include <fsfw/ipc/QueueFactory.h>
2021-06-08 13:35:49 +02:00
#include <fsfw/objectmanager/ObjectManager.h>
2022-05-05 20:55:28 +02:00
#include <fsfw/serviceinterface/ServiceInterface.h>
2021-06-08 13:35:49 +02:00
#include <fsfw/tasks/TaskFactory.h>
2022-05-05 20:55:28 +02:00
#include "OBSWConfig.h"
2022-05-20 09:16:59 +02:00
#include "commonObjects.h"
2022-05-05 20:55:28 +02:00
#include "objects/systemObjectList.h"
2021-06-08 13:35:49 +02:00
2022-05-05 20:55:28 +02:00
FsfwExampleTask::FsfwExampleTask(object_id_t objectId)
2022-05-22 15:30:08 +02:00
: SystemObject(objectId), poolManager(this, nullptr), demoSet(this),
monitor(objectId, MONITOR_ID,
gp_id_t(objectId, FsfwDemoSet::VARIABLE_LIMIT), 30, 10) {
commandQueue = QueueFactory::instance()->createMessageQueue(
10, CommandMessage::MAX_MESSAGE_SIZE);
2021-06-08 13:35:49 +02:00
}
2022-05-05 20:55:28 +02:00
FsfwExampleTask::~FsfwExampleTask() {}
2021-06-08 13:35:49 +02:00
ReturnValue_t FsfwExampleTask::performOperation(uint8_t operationCode) {
2022-05-05 20:55:28 +02:00
if (operationCode == OpCodes::DELAY_SHORT) {
TaskFactory::delayTask(5);
}
2021-06-08 13:35:49 +02:00
2022-05-05 20:55:28 +02:00
// TODO: Move this to new test controller?
ReturnValue_t result = performMonitoringDemo();
if (result != HasReturnvaluesIF::RETURN_OK) {
return result;
}
2021-06-08 13:35:49 +02:00
2022-05-05 20:55:28 +02:00
if (operationCode == OpCodes::SEND_RAND_NUM) {
result = performSendOperation();
if (result != HasReturnvaluesIF::RETURN_OK) {
return result;
2021-06-08 13:35:49 +02:00
}
2022-05-05 20:55:28 +02:00
}
2021-06-08 13:35:49 +02:00
2022-05-05 20:55:28 +02:00
if (operationCode == OpCodes::RECEIVE_RAND_NUM) {
result = performReceiveOperation();
}
2021-06-08 13:35:49 +02:00
2022-05-05 20:55:28 +02:00
return 0;
2021-06-08 13:35:49 +02:00
}
object_id_t FsfwExampleTask::getNextRecipient() {
2022-05-05 20:55:28 +02:00
switch (this->getObjectId()) {
2022-05-22 15:30:08 +02:00
case (objects::TEST_DUMMY_1): {
return objects::TEST_DUMMY_2;
}
case (objects::TEST_DUMMY_2): {
return objects::TEST_DUMMY_3;
}
case (objects::TEST_DUMMY_3): {
return objects::TEST_DUMMY_1;
}
default:
return objects::TEST_DUMMY_1;
2022-05-05 20:55:28 +02:00
}
2021-06-08 13:35:49 +02:00
}
object_id_t FsfwExampleTask::getSender() {
2022-05-05 20:55:28 +02:00
switch (this->getObjectId()) {
2022-05-22 15:30:08 +02:00
case (objects::TEST_DUMMY_1): {
return objects::TEST_DUMMY_3;
}
case (objects::TEST_DUMMY_2): {
return objects::TEST_DUMMY_1;
}
case (objects::TEST_DUMMY_3): {
return objects::TEST_DUMMY_2;
}
default:
return objects::TEST_DUMMY_1;
2022-05-05 20:55:28 +02:00
}
2021-06-08 13:35:49 +02:00
}
ReturnValue_t FsfwExampleTask::initialize() {
2022-05-05 20:55:28 +02:00
// Get the dataset of the sender. Will be cached for later checks.
object_id_t sender = getSender();
2022-05-22 15:30:08 +02:00
HasLocalDataPoolIF *senderIF =
ObjectManager::instance()->get<HasLocalDataPoolIF>(sender);
2022-05-05 20:55:28 +02:00
if (senderIF == nullptr) {
2021-06-08 13:35:49 +02:00
#if FSFW_CPP_OSTREAM_ENABLED == 1
2022-05-22 15:30:08 +02:00
sif::error << "FsfwDemoTask::initialize: Sender object invalid!"
<< std::endl;
2021-06-08 13:35:49 +02:00
#else
2022-05-05 20:55:28 +02:00
sif::printError("FsfwDemoTask::initialize: Sender object invalid!\n");
2021-06-08 13:35:49 +02:00
#endif
2022-05-05 20:55:28 +02:00
return HasReturnvaluesIF::RETURN_FAILED;
}
2021-06-08 13:35:49 +02:00
2022-05-22 15:30:08 +02:00
// we need a private copy of the previous dataset.. or we use the shared
// dataset.
2022-05-05 20:55:28 +02:00
senderSet = new FsfwDemoSet(senderIF);
if (senderSet == nullptr) {
2021-06-08 13:35:49 +02:00
#if FSFW_CPP_OSTREAM_ENABLED == 1
2022-05-22 15:30:08 +02:00
sif::error << "FsfwDemoTask::initialize: Sender dataset invalid!"
<< std::endl;
2021-06-08 13:35:49 +02:00
#else
2022-05-05 20:55:28 +02:00
sif::printError("FsfwDemoTask::initialize: Sender dataset invalid!\n");
2021-06-08 13:35:49 +02:00
#endif
2022-05-05 20:55:28 +02:00
return HasReturnvaluesIF::RETURN_FAILED;
}
return poolManager.initialize(commandQueue);
2021-06-08 13:35:49 +02:00
}
ReturnValue_t FsfwExampleTask::initializeAfterTaskCreation() {
2022-05-05 20:55:28 +02:00
return poolManager.initializeAfterTaskCreation();
2021-06-08 13:35:49 +02:00
}
2022-05-22 15:30:08 +02:00
object_id_t FsfwExampleTask::getObjectId() const {
return SystemObject::getObjectId();
}
2021-06-08 13:35:49 +02:00
2022-05-22 15:30:08 +02:00
MessageQueueId_t FsfwExampleTask::getMessageQueueId() {
return commandQueue->getId();
}
2021-06-08 13:35:49 +02:00
2022-05-22 15:30:08 +02:00
void FsfwExampleTask::setTaskIF(PeriodicTaskIF *task) { this->task = task; }
2021-06-08 13:35:49 +02:00
2022-05-22 15:30:08 +02:00
LocalPoolDataSetBase *FsfwExampleTask::getDataSetHandle(sid_t sid) {
return &demoSet;
}
2021-06-08 13:35:49 +02:00
2022-05-22 15:30:08 +02:00
uint32_t FsfwExampleTask::getPeriodicOperationFrequency() const {
return task->getPeriodMs();
}
2021-06-08 13:35:49 +02:00
2022-05-22 15:30:08 +02:00
ReturnValue_t
FsfwExampleTask::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) {
localDataPoolMap.emplace(FsfwDemoSet::PoolIds::VARIABLE,
new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(FsfwDemoSet::PoolIds::VARIABLE_LIMIT,
new PoolEntry<uint16_t>({0}));
2022-05-05 20:55:28 +02:00
return HasReturnvaluesIF::RETURN_OK;
2021-06-08 13:35:49 +02:00
}
ReturnValue_t FsfwExampleTask::performMonitoringDemo() {
2022-05-22 15:30:08 +02:00
ReturnValue_t result =
demoSet.variableLimit.read(MutexIF::TimeoutType::WAITING, 20);
2022-05-05 20:55:28 +02:00
if (result != HasReturnvaluesIF::RETURN_OK) {
/* Configuration error */
2021-06-08 13:35:49 +02:00
#if FSFW_CPP_OSTREAM_ENABLED == 1
2022-05-22 15:30:08 +02:00
sif::error << "DummyObject::performOperation: Could not read variableLimit!"
<< std::endl;
2021-06-08 13:35:49 +02:00
#else
2022-05-22 15:30:08 +02:00
sif::printError(
"DummyObject::performOperation: Could not read variableLimit!\n");
2021-06-08 13:35:49 +02:00
#endif
2022-05-05 20:55:28 +02:00
return result;
}
if (this->getObjectId() == objects::TEST_DUMMY_5) {
if (demoSet.variableLimit.value > 20) {
demoSet.variableLimit.value = 0;
2021-06-08 13:35:49 +02:00
}
2022-05-05 20:55:28 +02:00
demoSet.variableLimit.value++;
demoSet.variableLimit.commit(20);
monitor.check();
}
return HasReturnvaluesIF::RETURN_OK;
2021-06-08 13:35:49 +02:00
}
ReturnValue_t FsfwExampleTask::performSendOperation() {
2022-05-05 20:55:28 +02:00
object_id_t nextRecipient = getNextRecipient();
2022-05-22 15:30:08 +02:00
FsfwExampleTask *target =
ObjectManager::instance()->get<FsfwExampleTask>(nextRecipient);
2022-05-05 20:55:28 +02:00
if (target == nullptr) {
/* Configuration error */
2021-06-08 13:35:49 +02:00
#if FSFW_CPP_OSTREAM_ENABLED == 1
2022-05-22 15:30:08 +02:00
sif::error
<< "DummyObject::performOperation: Next recipient does not exist!"
<< std::endl;
2021-06-08 13:35:49 +02:00
#else
2022-05-22 15:30:08 +02:00
sif::printError(
"DummyObject::performOperation: Next recipient does not exist!\n");
2021-06-08 13:35:49 +02:00
#endif
2022-05-05 20:55:28 +02:00
return HasReturnvaluesIF::RETURN_FAILED;
}
2021-06-08 13:35:49 +02:00
2022-05-05 20:55:28 +02:00
uint32_t randomNumber = rand() % 100;
CommandMessage message;
message.setParameter(randomNumber);
message.setParameter2(this->getMessageQueueId());
2021-06-08 13:35:49 +02:00
2022-05-05 20:55:28 +02:00
/* Send message using own message queue */
2022-05-22 15:30:08 +02:00
ReturnValue_t result =
commandQueue->sendMessage(target->getMessageQueueId(), &message);
if (result != HasReturnvaluesIF::RETURN_OK &&
result != MessageQueueIF::FULL) {
2021-06-08 13:35:49 +02:00
#if FSFW_CPP_OSTREAM_ENABLED == 1
2022-05-22 15:30:08 +02:00
sif::error << "FsfwDemoTask::performSendOperation: Send failed with "
<< result << std::endl;
2021-06-08 13:35:49 +02:00
#else
2022-05-22 15:30:08 +02:00
sif::printError(
"FsfwDemoTask::performSendOperation: Send failed with %hu\n", result);
2021-06-08 13:35:49 +02:00
#endif
2022-05-05 20:55:28 +02:00
}
2021-06-08 13:35:49 +02:00
2022-05-05 20:55:28 +02:00
/* Send message without via MessageQueueSenderIF */
2022-05-22 15:30:08 +02:00
result = MessageQueueSenderIF::sendMessage(target->getMessageQueueId(),
&message, commandQueue->getId());
if (result != HasReturnvaluesIF::RETURN_OK &&
result != MessageQueueIF::FULL) {
2021-06-08 13:35:49 +02:00
#if FSFW_CPP_OSTREAM_ENABLED == 1
2022-05-22 15:30:08 +02:00
sif::error << "FsfwDemoTask::performSendOperation: Send failed with "
<< result << std::endl;
2021-06-08 13:35:49 +02:00
#else
2022-05-22 15:30:08 +02:00
sif::printError(
"FsfwDemoTask::performSendOperation: Send failed with %hu\n", result);
2021-06-08 13:35:49 +02:00
#endif
2022-05-05 20:55:28 +02:00
}
2021-06-08 13:35:49 +02:00
2022-05-05 20:55:28 +02:00
demoSet.variableWrite.value = randomNumber;
result = demoSet.variableWrite.commit(20);
2021-06-08 13:35:49 +02:00
2022-05-05 20:55:28 +02:00
return result;
2021-06-08 13:35:49 +02:00
}
ReturnValue_t FsfwExampleTask::performReceiveOperation() {
2022-05-05 20:55:28 +02:00
ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
while (result != MessageQueueIF::EMPTY) {
CommandMessage receivedMessage;
result = commandQueue->receiveMessage(&receivedMessage);
2022-05-22 15:30:08 +02:00
if (result != HasReturnvaluesIF::RETURN_OK &&
result != MessageQueueIF::EMPTY) {
2021-06-08 13:35:49 +02:00
#if FSFW_CPP_OSTREAM_ENABLED == 1
2022-05-05 20:55:28 +02:00
sif::debug << "Receive failed with " << result << std::endl;
2021-06-08 13:35:49 +02:00
#endif
2022-05-05 20:55:28 +02:00
break;
}
if (result != MessageQueueIF::EMPTY) {
2021-06-08 13:35:49 +02:00
#if FSFW_CPP_OSTREAM_ENABLED == 1
#if OBSW_VERBOSE_LEVEL >= 2
2022-05-05 20:55:28 +02:00
sif::debug << "Message Received by " << getObjectId() << " from Queue "
2022-05-22 15:30:08 +02:00
<< receivedMessage.getSender() << " ObjectId "
<< receivedMessage.getParameter() << " Queue "
<< receivedMessage.getParameter2() << std::endl;
2021-06-08 13:35:49 +02:00
#endif
#endif
2022-05-05 20:55:28 +02:00
if (senderSet == nullptr) {
return HasReturnvaluesIF::RETURN_FAILED;
}
2021-06-08 13:35:49 +02:00
2022-05-05 20:55:28 +02:00
result = senderSet->variableRead.read(MutexIF::TimeoutType::WAITING, 20);
if (result != HasReturnvaluesIF::RETURN_OK) {
return result;
}
if (senderSet->variableRead.value != receivedMessage.getParameter()) {
2021-06-08 13:35:49 +02:00
#if FSFW_CPP_OSTREAM_ENABLED == 1
2022-05-22 15:30:08 +02:00
sif::error << "FsfwDemoTask::performReceiveOperation: Variable "
<< std::hex << "0x"
<< senderSet->variableRead.getDataPoolId() << std::dec
<< " has wrong value." << std::endl;
2022-05-05 20:55:28 +02:00
sif::error << "Value: " << demoSet.variableRead.value
2022-05-22 15:30:08 +02:00
<< ", expected: " << receivedMessage.getParameter()
<< std::endl;
2021-06-08 13:35:49 +02:00
#endif
2022-05-05 20:55:28 +02:00
}
2021-06-08 13:35:49 +02:00
}
2022-05-05 20:55:28 +02:00
}
return result;
2021-06-08 13:35:49 +02:00
}
2022-05-22 15:30:08 +02:00
MessageQueueId_t FsfwExampleTask::getCommandQueue() const {
return commandQueue->getId();
}
2021-06-08 13:35:49 +02:00
2022-05-22 15:30:08 +02:00
LocalDataPoolManager *FsfwExampleTask::getHkManagerHandle() {
return &poolManager;
}