eive-obsw/mission/system/objects/AcsSubsystem.cpp

103 lines
4.0 KiB
C++
Raw Normal View History

#include "AcsSubsystem.h"
2022-04-14 09:54:07 +02:00
#include <fsfw/events/EventManagerIF.h>
#include <fsfw/ipc/QueueFactory.h>
#include "fsfw/modes/ModeMessage.h"
#include "mission/acsDefs.h"
2022-09-29 19:40:00 +02:00
AcsSubsystem::AcsSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences,
uint32_t maxNumberOfTables)
: Subsystem(setObjectId, maxNumberOfSequences, maxNumberOfTables) {
auto mqArgs = MqArgs(getObjectId(), static_cast<void*>(this));
eventQueue =
QueueFactory::instance()->createMessageQueue(10, EventMessage::EVENT_MESSAGE_SIZE, &mqArgs);
}
ReturnValue_t AcsSubsystem::initialize() {
EventManagerIF* 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;
;
}
2023-02-03 10:52:39 +01:00
result =
manager->subscribeToEvent(eventQueue->getId(), event::getEventId(acs::SAFE_RATE_VIOLATION));
if (result != returnvalue::OK) {
sif::error << "AcsSubsystem: Subscribing for acs::SAFE_RATE_VIOLATION failed" << std::endl;
}
2023-02-03 10:52:39 +01:00
result =
manager->subscribeToEvent(eventQueue->getId(), event::getEventId(acs::SAFE_RATE_RECOVERY));
if (result != returnvalue::OK) {
sif::error << "AcsSubsystem: Subscribing for acs::SAFE_RATE_RECOVERY failed" << std::endl;
}
result =
manager->subscribeToEvent(eventQueue->getId(), event::getEventId(acs::MULTIPLE_RW_INVALID));
if (result != returnvalue::OK) {
sif::error << "AcsSubsystem: Subscribing for acs::MULTIPLE_RW_INVALID failed" << std::endl;
}
2023-02-22 15:02:15 +01:00
result = manager->subscribeToEvent(eventQueue->getId(),
event::getEventId(acs::MEKF_INVALID_MODE_VIOLATION));
if (result != returnvalue::OK) {
sif::error << "AcsSubsystem: Subscribing for acs::MULTIPLE_RW_INVALID failed" << std::endl;
}
return Subsystem::initialize();
}
void AcsSubsystem::performChildOperation() {
2023-02-03 10:52:39 +01:00
handleEventMessages();
return Subsystem::performChildOperation();
}
void AcsSubsystem::handleEventMessages() {
EventMessage event;
for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == returnvalue::OK;
result = eventQueue->receiveMessage(&event)) {
2023-02-17 12:19:53 +01:00
ReturnValue_t status;
switch (event.getMessageId()) {
case EventMessage::EVENT_MESSAGE:
if (event.getEvent() == acs::SAFE_RATE_VIOLATION) {
CommandMessage msg;
2023-02-08 13:18:45 +01:00
ModeMessage::setCmdModeMessage(msg, acs::AcsMode::DETUMBLE, 0);
2023-02-17 12:19:53 +01:00
status = commandQueue->sendMessage(commandQueue->getId(), &msg);
if (result != returnvalue::OK) {
sif::error << "AcsSubsystem: sending DETUMBLE mode cmd to self has failed" << std::endl;
}
}
if (event.getEvent() == acs::SAFE_RATE_RECOVERY ||
2023-02-22 15:02:15 +01:00
event.getEvent() == acs::MULTIPLE_RW_INVALID ||
event.getEvent() == acs::MEKF_INVALID_MODE_VIOLATION) {
CommandMessage msg;
2023-02-08 13:18:45 +01:00
ModeMessage::setCmdModeMessage(msg, acs::AcsMode::SAFE, 0);
2023-02-17 12:19:53 +01:00
status = commandQueue->sendMessage(commandQueue->getId(), &msg);
if (status != returnvalue::OK) {
sif::error << "AcsSubsystem: sending SAFE mode cmd to self has failed" << std::endl;
}
}
break;
default:
sif::debug << "AcsSubsystem::performChildOperation: Did not subscribe "
"to this event message"
<< std::endl;
break;
}
}
}
2023-02-13 01:26:30 +01:00
void AcsSubsystem::announceMode(bool recursive) {
const char* modeStr = acs::getModeStr(static_cast<acs::AcsMode>(mode));
sif::info << "ACS subsystem is now in " << modeStr << " mode" << std::endl;
return Subsystem::announceMode(recursive);
}