2021-07-13 20:22:54 +02:00
|
|
|
#include "fsfw/controller/ControllerBase.h"
|
2020-12-03 13:14:30 +01:00
|
|
|
|
2021-07-13 20:22:54 +02:00
|
|
|
#include "fsfw/action/HasActionsIF.h"
|
2022-02-02 10:29:30 +01:00
|
|
|
#include "fsfw/ipc/QueueFactory.h"
|
2021-07-13 20:22:54 +02:00
|
|
|
#include "fsfw/objectmanager/ObjectManager.h"
|
2022-02-02 10:29:30 +01:00
|
|
|
#include "fsfw/subsystem/SubsystemBase.h"
|
2022-09-29 19:21:24 +02:00
|
|
|
#include "fsfw/subsystem/helper.h"
|
2016-06-15 23:48:41 +02:00
|
|
|
|
2022-09-29 19:21:24 +02:00
|
|
|
ControllerBase::ControllerBase(object_id_t setObjectId, size_t commandQueueDepth)
|
2022-02-02 10:29:30 +01:00
|
|
|
: SystemObject(setObjectId),
|
|
|
|
mode(MODE_OFF),
|
|
|
|
submode(SUBMODE_NONE),
|
|
|
|
modeHelper(this),
|
|
|
|
healthHelper(this, setObjectId) {
|
2022-02-19 16:14:02 +01:00
|
|
|
auto mqArgs = MqArgs(setObjectId, static_cast<void*>(this));
|
2022-02-22 10:17:56 +01:00
|
|
|
commandQueue = QueueFactory::instance()->createMessageQueue(
|
|
|
|
commandQueueDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
|
2016-06-15 23:48:41 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ControllerBase::~ControllerBase() { QueueFactory::instance()->deleteMessageQueue(commandQueue); }
|
2016-06-15 23:48:41 +02:00
|
|
|
|
|
|
|
ReturnValue_t ControllerBase::initialize() {
|
2022-09-30 14:14:45 +02:00
|
|
|
ReturnValue_t result = modeHelper.initialize();
|
|
|
|
if(result != returnvalue::OK) {
|
2022-02-02 10:29:30 +01:00
|
|
|
return result;
|
|
|
|
}
|
2022-09-30 14:14:45 +02:00
|
|
|
result = healthHelper.initialize();
|
|
|
|
if(result != returnvalue::OK) {
|
|
|
|
return result;
|
|
|
|
}
|
|
|
|
return SystemObject::initialize();
|
2016-06-15 23:48:41 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
MessageQueueId_t ControllerBase::getCommandQueue() const { return commandQueue->getId(); }
|
2016-06-15 23:48:41 +02:00
|
|
|
|
|
|
|
void ControllerBase::handleQueue() {
|
2022-02-02 10:29:30 +01:00
|
|
|
CommandMessage command;
|
2022-07-04 11:48:36 +02:00
|
|
|
ReturnValue_t result;
|
2022-08-16 01:08:26 +02:00
|
|
|
for (result = commandQueue->receiveMessage(&command); result == returnvalue::OK;
|
2022-02-02 10:29:30 +01:00
|
|
|
result = commandQueue->receiveMessage(&command)) {
|
|
|
|
result = modeHelper.handleModeCommand(&command);
|
2022-08-16 01:08:26 +02:00
|
|
|
if (result == returnvalue::OK) {
|
2022-02-02 10:29:30 +01:00
|
|
|
continue;
|
2021-01-28 11:50:33 +01:00
|
|
|
}
|
2016-06-15 23:48:41 +02:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
result = healthHelper.handleHealthCommand(&command);
|
2022-08-16 01:08:26 +02:00
|
|
|
if (result == returnvalue::OK) {
|
2022-02-02 10:29:30 +01:00
|
|
|
continue;
|
|
|
|
}
|
|
|
|
result = handleCommandMessage(&command);
|
2022-08-16 01:08:26 +02:00
|
|
|
if (result == returnvalue::OK) {
|
2022-02-02 10:29:30 +01:00
|
|
|
continue;
|
|
|
|
}
|
|
|
|
command.setToUnknownCommand();
|
|
|
|
commandQueue->reply(&command);
|
|
|
|
}
|
2016-06-15 23:48:41 +02:00
|
|
|
}
|
|
|
|
|
2022-07-04 11:48:36 +02:00
|
|
|
void ControllerBase::startTransition(Mode_t mode_, Submode_t submode_) {
|
2022-02-02 10:29:30 +01:00
|
|
|
changeHK(this->mode, this->submode, false);
|
|
|
|
triggerEvent(CHANGING_MODE, mode, submode);
|
2022-07-04 11:48:36 +02:00
|
|
|
mode = mode_;
|
|
|
|
submode = submode_;
|
2022-02-02 10:29:30 +01:00
|
|
|
modeHelper.modeChanged(mode, submode);
|
|
|
|
modeChanged(mode, submode);
|
|
|
|
announceMode(false);
|
|
|
|
changeHK(this->mode, this->submode, true);
|
2016-06-15 23:48:41 +02:00
|
|
|
}
|
|
|
|
|
2022-07-04 11:48:36 +02:00
|
|
|
void ControllerBase::getMode(Mode_t* mode_, Submode_t* submode_) {
|
|
|
|
*mode_ = this->mode;
|
|
|
|
*submode_ = this->submode;
|
2016-06-15 23:48:41 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
void ControllerBase::setToExternalControl() { healthHelper.setHealth(EXTERNAL_CONTROL); }
|
2016-06-15 23:48:41 +02:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
void ControllerBase::announceMode(bool recursive) { triggerEvent(MODE_INFO, mode, submode); }
|
2016-06-15 23:48:41 +02:00
|
|
|
|
2018-07-12 16:29:32 +02:00
|
|
|
ReturnValue_t ControllerBase::performOperation(uint8_t opCode) {
|
2022-02-02 10:29:30 +01:00
|
|
|
handleQueue();
|
|
|
|
performControlOperation();
|
2022-08-16 01:08:26 +02:00
|
|
|
return returnvalue::OK;
|
2016-06-15 23:48:41 +02:00
|
|
|
}
|
|
|
|
|
2022-07-04 11:48:36 +02:00
|
|
|
void ControllerBase::modeChanged(Mode_t mode_, Submode_t submode_) {}
|
2016-06-15 23:48:41 +02:00
|
|
|
|
|
|
|
ReturnValue_t ControllerBase::setHealth(HealthState health) {
|
2022-02-02 10:29:30 +01:00
|
|
|
switch (health) {
|
2021-01-28 11:50:33 +01:00
|
|
|
case HEALTHY:
|
|
|
|
case EXTERNAL_CONTROL:
|
2022-02-02 10:29:30 +01:00
|
|
|
healthHelper.setHealth(health);
|
2022-08-16 01:08:26 +02:00
|
|
|
return returnvalue::OK;
|
2021-01-28 11:50:33 +01:00
|
|
|
default:
|
2022-02-02 10:29:30 +01:00
|
|
|
return INVALID_HEALTH_STATE;
|
|
|
|
}
|
2016-06-15 23:48:41 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
HasHealthIF::HealthState ControllerBase::getHealth() { return healthHelper.getHealth(); }
|
|
|
|
void ControllerBase::setTaskIF(PeriodicTaskIF* task_) { executingTask = task_; }
|
2018-07-12 16:29:32 +02:00
|
|
|
|
2022-07-04 11:48:36 +02:00
|
|
|
void ControllerBase::changeHK(Mode_t mode_, Submode_t submode_, bool enable) {}
|
2020-12-03 13:14:30 +01:00
|
|
|
|
2022-08-16 01:08:26 +02:00
|
|
|
ReturnValue_t ControllerBase::initializeAfterTaskCreation() { return returnvalue::OK; }
|
2022-09-29 19:21:24 +02:00
|
|
|
|
|
|
|
const HasHealthIF* ControllerBase::getOptHealthIF() const { return this; }
|
|
|
|
|
|
|
|
const HasModesIF& ControllerBase::getModeIF() const { return *this; }
|
|
|
|
|
2022-09-30 13:30:07 +02:00
|
|
|
ModeTreeChildIF& ControllerBase::getModeTreeChildIF() {
|
|
|
|
return *this;
|
|
|
|
}
|
|
|
|
|
2022-09-29 19:39:37 +02:00
|
|
|
ReturnValue_t ControllerBase::connectModeTreeParent(HasModeTreeChildrenIF& parent) {
|
|
|
|
return modetree::connectModeTreeParent(parent, *this, healthHelper, modeHelper);
|
2022-09-29 19:21:24 +02:00
|
|
|
}
|