Robin Mueller
350ed59033
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
112 lines
3.5 KiB
C++
112 lines
3.5 KiB
C++
#include "EiveSystem.h"
|
|
|
|
#include <eive/objects.h>
|
|
#include <fsfw/events/EventManager.h>
|
|
#include <fsfw/ipc/QueueFactory.h>
|
|
#include <mission/acsDefs.h>
|
|
#include <mission/comDefs.h>
|
|
#include <mission/controller/controllerdefinitions/tcsCtrlDefs.h>
|
|
|
|
EiveSystem::EiveSystem(object_id_t setObjectId, uint32_t maxNumberOfSequences,
|
|
uint32_t maxNumberOfTables)
|
|
: Subsystem(setObjectId, maxNumberOfSequences, maxNumberOfTables) {
|
|
auto mqArgs = MqArgs(SubsystemBase::getObjectId(), static_cast<void*>(this));
|
|
eventQueue =
|
|
QueueFactory::instance()->createMessageQueue(10, EventMessage::EVENT_MESSAGE_SIZE, &mqArgs);
|
|
}
|
|
|
|
void EiveSystem::announceMode(bool recursive) {
|
|
const char* modeStr = "UNKNOWN";
|
|
switch (mode) {
|
|
case (acs::AcsMode::OFF): {
|
|
modeStr = "OFF";
|
|
break;
|
|
}
|
|
case (acs::AcsMode::SAFE): {
|
|
modeStr = "SAFE";
|
|
break;
|
|
}
|
|
case (acs::AcsMode::PTG_IDLE): {
|
|
modeStr = "POINTING IDLE";
|
|
break;
|
|
}
|
|
case (acs::AcsMode::PTG_INERTIAL): {
|
|
modeStr = "POINTING INERTIAL";
|
|
break;
|
|
}
|
|
case (acs::AcsMode::PTG_TARGET): {
|
|
modeStr = "POINTING TARGET";
|
|
break;
|
|
}
|
|
case (acs::AcsMode::PTG_TARGET_GS): {
|
|
modeStr = "POINTING TARGET GS";
|
|
break;
|
|
}
|
|
}
|
|
sif::info << "EIVE system is now in " << modeStr << " mode" << std::endl;
|
|
return Subsystem::announceMode(recursive);
|
|
}
|
|
|
|
void EiveSystem::performChildOperation() {
|
|
handleEventMessages();
|
|
return Subsystem::performChildOperation();
|
|
}
|
|
|
|
ReturnValue_t EiveSystem::initialize() {
|
|
auto* manager = ObjectManager::instance()->get<EventManagerIF>(objects::EVENT_MANAGER);
|
|
if (manager == nullptr) {
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "AcsSubsystem::initialize: Invalid event manager" << std::endl;
|
|
#endif
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
}
|
|
ReturnValue_t result = manager->registerListener(eventQueue->getId());
|
|
if (result != returnvalue::OK) {
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::warning << "AcsSubsystem::registerListener: Failed to register as "
|
|
"listener"
|
|
<< std::endl;
|
|
#endif
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
}
|
|
manager->subscribeToEvent(eventQueue->getId(),
|
|
event::getEventId(tcsCtrl::PCDU_SYSTEM_OVERHEATING));
|
|
manager->subscribeToEvent(eventQueue->getId(), event::getEventId(tcsCtrl::OBC_OVERHEATING));
|
|
|
|
return Subsystem::initialize();
|
|
}
|
|
|
|
void EiveSystem::handleEventMessages() {
|
|
EventMessage event;
|
|
for (ReturnValue_t status = eventQueue->receiveMessage(&event); status == returnvalue::OK;
|
|
status = eventQueue->receiveMessage(&event)) {
|
|
switch (event.getMessageId()) {
|
|
case EventMessage::EVENT_MESSAGE:
|
|
switch (event.getEvent()) {
|
|
case tcsCtrl::OBC_OVERHEATING:
|
|
case tcsCtrl::PCDU_SYSTEM_OVERHEATING: {
|
|
commandSelfToSafe();
|
|
break;
|
|
}
|
|
}
|
|
break;
|
|
default:
|
|
sif::debug << "AcsSubsystem::performChildOperation: Did not subscribe "
|
|
"to this event message"
|
|
<< std::endl;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
void EiveSystem::commandSelfToSafe() {
|
|
if (fallbackCommandCd.hasTimedOut()) {
|
|
CommandMessage msg;
|
|
ModeMessage::setCmdModeMessage(msg, acs::AcsMode::SAFE, 0);
|
|
ReturnValue_t result = commandQueue->sendMessage(commandQueue->getId(), &msg);
|
|
if (result != returnvalue::OK) {
|
|
sif::error << "EiveSystem: Commanding EIVE-System to SAFE failed" << std::endl;
|
|
}
|
|
fallbackCommandCd.resetTimer();
|
|
}
|
|
}
|