eive-obsw/mission/system/objects/ComSubsystem.cpp
2023-02-22 09:53:09 +01:00

179 lines
6.2 KiB
C++

#include <utility>
#include "ComSubsystem.h"
#include <fsfw/events/EventManagerIF.h>
#include <fsfw/ipc/MutexGuard.h>
#include <fsfw/ipc/QueueFactory.h>
#include <fsfw/serviceinterface/ServiceInterface.h>
#include <linux/ipcore/PdecHandler.h>
#include <mission/config/comCfg.h>
extern std::pair<uint8_t, FixedArrayList<ModeListEntry, 3>> COM_SEQUENCE_RX_ONLY;
//extern std::pair<uint32_t, FixedArrayList<ModeListEntry, 3>> COM_TABLE_RX_ONLY_TGT;
extern std::pair<uint8_t, FixedArrayList<ModeListEntry, 3>> COM_SEQUENCE_RX_AND_TX_DEFAULT_RATE;
extern std::pair<uint32_t, FixedArrayList<ModeListEntry, 3>> COM_TABLE_RX_AND_TX_DEFAULT_RATE_TGT;
extern std::pair<uint8_t, FixedArrayList<ModeListEntry, 3>> COM_SEQUENCE_RX_AND_TX_LOW_RATE;
extern std::pair<uint8_t, FixedArrayList<ModeListEntry, 3>> COM_SEQUENCE_RX_AND_TX_HIGH_RATE;
ComSubsystem::ComSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences,
uint32_t maxNumberOfTables, uint32_t transmitterTimeout)
: Subsystem(setObjectId, maxNumberOfSequences, maxNumberOfTables), paramHelper(this) {
com::setCurrentDatarate(com::Datarate::LOW_RATE_MODULATION_BPSK);
auto mqArgs = MqArgs(setObjectId, static_cast<void *>(this));
eventQueue =
QueueFactory::instance()->createMessageQueue(10, EventMessage::EVENT_MESSAGE_SIZE, &mqArgs);
transmitterCountdown.setTimeout(transmitterTimeout);
}
void ComSubsystem::performChildOperation() {
readEventQueue();
if (countdownActive) {
checkTransmitterCountdown();
}
Subsystem::performChildOperation();
}
MessageQueueId_t ComSubsystem::getCommandQueue() const { return Subsystem::getCommandQueue(); }
ReturnValue_t ComSubsystem::getParameter(uint8_t domainId, uint8_t uniqueIdentifier,
ParameterWrapper *parameterWrapper,
const ParameterWrapper *newValues, uint16_t startAtIndex) {
if ((domainId == 0) and (uniqueIdentifier == static_cast<uint8_t>(com::ParameterId::DATARATE))) {
uint8_t newVal = 0;
ReturnValue_t result = newValues->getElement(&newVal);
if (result != returnvalue::OK) {
return result;
}
if (newVal >= static_cast<uint8_t>(com::Datarate::NUM_DATARATES)) {
return HasParametersIF::INVALID_VALUE;
}
parameterWrapper->set(datarateCfg);
com::setCurrentDatarate(static_cast<com::Datarate>(newVal));
return returnvalue::OK;
}
else if ((domainId == 0) and (uniqueIdentifier == static_cast<uint8_t>(com::ParameterId::TRANSMITTER_TIMEOUT))) {
uint8_t newVal = 0;
ReturnValue_t result = newValues->getElement(&newVal);
if (result != returnvalue::OK) {
return result;
}
parameterWrapper->set(transmitterTimeout);
transmitterTimeout = newVal;
transmitterCountdown.setTimeout(transmitterTimeout);
return returnvalue::OK;
}
return returnvalue::OK;
}
ReturnValue_t ComSubsystem::handleCommandMessage(CommandMessage *message) {
ReturnValue_t result = paramHelper.handleParameterMessage(message);
if (result == returnvalue::OK) {
return result;
}
return Subsystem::handleCommandMessage(message);
}
ReturnValue_t ComSubsystem::initialize() {
ReturnValue_t result = paramHelper.initialize();
if (result != returnvalue::OK) {
return result;
}
EventManagerIF* manager = ObjectManager::instance()->get<EventManagerIF>(objects::EVENT_MANAGER);
if (manager == nullptr) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "ComSubsystem::initialize: Invalid event manager" << std::endl;
#endif
return ObjectManagerIF::CHILD_INIT_FAILED;
}
result = manager->registerListener(eventQueue->getId());
if (result != returnvalue::OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::warning << "ComSubsystem::initialize: Failed to register Com Subsystem as event "
"listener" << std::endl;
#endif
return ObjectManagerIF::CHILD_INIT_FAILED;
}
result = manager->subscribeToEventRange(eventQueue->getId(),
event::getEventId(PdecHandler::CARRIER_LOCK),
event::getEventId(PdecHandler::BIT_LOCK_PDEC));
if (result != returnvalue::OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "ComSubsystem::initialize: Failed to subscribe to events from PDEC "
"handler"
<< std::endl;
#endif
return result;
}
return Subsystem::initialize();
}
void ComSubsystem::startTransition(Mode_t mode, Submode_t submode) {
// Depending on the submode the transmitter timeout is enabled or
// disabled here
if (mode == COM_SEQUENCE_RX_ONLY.first) {
transmitterCountdown.timeOut();
countdownActive = false;
} else if ((mode == COM_SEQUENCE_RX_AND_TX_DEFAULT_RATE.first) ||
(mode == COM_SEQUENCE_RX_AND_TX_LOW_RATE.first) ||
(mode == COM_SEQUENCE_RX_AND_TX_HIGH_RATE.first)) {
transmitterCountdown.resetTimer();
countdownActive = true;
}
Subsystem::startTransition(mode, submode);
}
void ComSubsystem::readEventQueue() {
EventMessage event;
for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == returnvalue::OK;
result = eventQueue->receiveMessage(&event)) {
switch (event.getMessageId()) {
case EventMessage::EVENT_MESSAGE:
handleEventMessage(&event);
break;
default:
sif::debug << "CcsdsHandler::checkEvents: Did not subscribe to this event message"
<< std::endl;
break;
}
}
}
void ComSubsystem::handleEventMessage(EventMessage *eventMessage) {
Event event = eventMessage->getEvent();
switch (event) {
case PdecHandler::BIT_LOCK_PDEC: {
handleBitLockEvent();
break;
}
case PdecHandler::CARRIER_LOCK: {
handleCarrierLockEvent();
break;
}
default:
sif::debug << "ComSubsystem::handleEvent: Did not subscribe to this event" << std::endl;
break;
}
}
void ComSubsystem::handleBitLockEvent() { startRxAndTxDefaultSeq(); }
void ComSubsystem::handleCarrierLockEvent() {
if (!enableTxWhenCarrierLock) {
return;
}
startRxAndTxDefaultSeq();
}
void ComSubsystem::startRxAndTxDefaultSeq() {
// Turns transmitter on
startTransition(COM_SEQUENCE_RX_AND_TX_DEFAULT_RATE.first, SUBMODE_NONE);
}
void ComSubsystem::checkTransmitterCountdown() {
if (transmitterCountdown.hasTimedOut()) {
startTransition(COM_SEQUENCE_RX_ONLY.first, SUBMODE_NONE);
countdownActive = false;
}
}