done

mission/system/com/ComSubsystem.cpp

@@ -0,0 +1,201 @@
+#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/com/defs.h>
+#include <mission/config/comCfg.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() {
+  readEventQueue();
+  // Execute default rate sequence after transition has been completed
+  if (rememberBitLock and not isInTransition) {
+    startRxAndTxLowRateSeq();
+    rememberBitLock = false;
+  }
+  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))) {
+    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
+    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::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 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() {
+  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;
+  }
+}
+
+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;
+}