255 lines
7.9 KiB
C++
255 lines
7.9 KiB
C++
#include "ComSubsystem.h"
|
|
|
|
#include <fsfw/events/Event.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/com/defs.h>
|
|
#include <mission/config/comCfg.h>
|
|
#include <mission/controller/tcsDefs.h>
|
|
|
|
#include <utility>
|
|
|
|
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() {
|
|
Subsystem::performChildOperation();
|
|
readEventQueue();
|
|
if (performRecoveryToRxOnly and not isInTransition) {
|
|
startRxOnlyRecovery(true);
|
|
// To avoid immediately enabling TX after falling back.
|
|
rememberBitLock = false;
|
|
}
|
|
// Execute default rate sequence after transition has been completed
|
|
if (rememberBitLock and not isInTransition) {
|
|
startRxAndTxLowRateSeq();
|
|
rememberBitLock = false;
|
|
}
|
|
if (countdownActive) {
|
|
checkTransmitterCountdown();
|
|
}
|
|
}
|
|
|
|
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))) {
|
|
uint32_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
|
|
}
|
|
result = manager->subscribeToEvent(eventQueue->getId(),
|
|
event::getEventId(tcsCtrl::SYRLINKS_OVERHEATING));
|
|
if (result != returnvalue::OK) {
|
|
return ObjectManager::CHILD_INIT_FAILED;
|
|
}
|
|
result =
|
|
manager->subscribeToEventRange(eventQueue->getId(), event::getEventId(pdec::CARRIER_LOCK),
|
|
event::getEventId(pdec::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::Submode::RX_ONLY) {
|
|
transmitterCountdown.timeOut();
|
|
countdownActive = false;
|
|
} else if (isTxMode(mode)) {
|
|
// Only start transmitter countdown if transmitter is not already on
|
|
if (not isTxMode(this->mode)) {
|
|
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 tcsCtrl::SYRLINKS_OVERHEATING: {
|
|
// This event overrides the bit lock.
|
|
rememberBitLock = false;
|
|
if (mode == com::RX_ONLY) {
|
|
return;
|
|
}
|
|
if (isInTransition) {
|
|
performRecoveryToRxOnly = true;
|
|
return;
|
|
}
|
|
startRxOnlyRecovery(true);
|
|
break;
|
|
}
|
|
case pdec::BIT_LOCK_PDEC: {
|
|
handleBitLockEvent();
|
|
break;
|
|
}
|
|
case pdec::CARRIER_LOCK: {
|
|
handleCarrierLockEvent();
|
|
break;
|
|
}
|
|
default:
|
|
sif::debug << "ComSubsystem::handleEvent: Did not subscribe to this event" << std::endl;
|
|
break;
|
|
}
|
|
}
|
|
|
|
void ComSubsystem::handleBitLockEvent() {
|
|
if (isTxMode(mode)) {
|
|
// Tx already on
|
|
return;
|
|
}
|
|
if (isInTransition) {
|
|
rememberBitLock = true;
|
|
return;
|
|
}
|
|
triggerEvent(BIT_LOCK_TX_ON);
|
|
startRxAndTxLowRateSeq();
|
|
}
|
|
|
|
void ComSubsystem::handleCarrierLockEvent() {
|
|
if (!enableTxWhenCarrierLock) {
|
|
return;
|
|
}
|
|
startRxAndTxLowRateSeq();
|
|
}
|
|
|
|
void ComSubsystem::startRxAndTxLowRateSeq() {
|
|
// Turns transmitter on
|
|
startTransition(com::Submode::RX_AND_TX_LOW_DATARATE, SUBMODE_NONE);
|
|
}
|
|
|
|
void ComSubsystem::checkTransmitterCountdown() {
|
|
if (transmitterCountdown.hasTimedOut()) {
|
|
triggerEvent(TX_TIMER_EXPIRED, transmitterTimeout);
|
|
startTransition(com::Submode::RX_ONLY, SUBMODE_NONE);
|
|
countdownActive = false;
|
|
}
|
|
}
|
|
|
|
void ComSubsystem::startRxOnlyRecovery(bool forced) {
|
|
modeHelper.setForced(forced);
|
|
startTransition(com::RX_ONLY, 0);
|
|
performRecoveryToRxOnly = false;
|
|
}
|
|
|
|
bool ComSubsystem::isTxMode(Mode_t mode) {
|
|
if ((mode == com::Submode::RX_AND_TX_DEFAULT_DATARATE) ||
|
|
(mode == com::Submode::RX_AND_TX_LOW_DATARATE) ||
|
|
(mode == com::Submode::RX_AND_TX_HIGH_DATARATE)) {
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void ComSubsystem::announceMode(bool recursive) {
|
|
const char *modeStr = "UNKNOWN";
|
|
switch (mode) {
|
|
case (com::RX_ONLY): {
|
|
modeStr = "RX_ONLY";
|
|
break;
|
|
}
|
|
case (com::RX_AND_TX_LOW_DATARATE): {
|
|
modeStr = "RX_AND_TX_LOW_DATARATE";
|
|
break;
|
|
}
|
|
case (com::RX_AND_TX_HIGH_DATARATE): {
|
|
modeStr = "RX_AND_TX_HIGH_DATARATE";
|
|
break;
|
|
}
|
|
case (com::RX_AND_TX_DEFAULT_DATARATE): {
|
|
modeStr = "RX_AND_TX_DEFAULT_DATARATE";
|
|
break;
|
|
}
|
|
}
|
|
sif::info << "COM subsystem is now in " << modeStr << " mode" << std::endl;
|
|
return Subsystem::announceMode(recursive);
|
|
}
|