CHANGELOG.md

@@ -14,6 +14,11 @@ list yields a list of all related PRs for each release.
   PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/310
 - Extended TM funnels to allow multiple TM recipients.
   PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/312
+- DHB: Transitions to normal mode now possible directly, which simplifies subsystem implementations
+  PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/313
+- MAX3185 Low Level Handler and Device Handler: Simplifications and bugfixes to allow switching
+  off without triggering unrequested replies
+  PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/313
 
 # [v1.15.0] 27.10.2022
 

fsfw

@@ -1 +1 @@
-Subproject commit 91ebf98c2851b0470a912c1c3279cb20e10a5490
+Subproject commit 84b9d1ce216c076bdfeeaf1663aa873c7f1c9cff

linux/devices/Max31865RtdLowlevelHandler.cpp

@@ -50,7 +50,7 @@ ReturnValue_t Max31865RtdReader::performOperation(uint8_t operationCode) {
 }
 
 bool Max31865RtdReader::rtdIsActive(uint8_t idx) {
-  if (rtds[idx]->on and rtds[idx]->active and rtds[idx]->configured) {
+  if (rtds[idx]->on and rtds[idx]->db.active and rtds[idx]->db.configured) {
     return true;
   }
   return false;
@@ -69,7 +69,7 @@ bool Max31865RtdReader::periodicInitHandling() {
     if (rtd == nullptr) {
       continue;
     }
-    if ((rtd->on or rtd->active) and not rtd->configured and rtd->cd.hasTimedOut()) {
+    if ((rtd->on or rtd->db.active) and not rtd->db.configured and rtd->cd.hasTimedOut()) {
       ManualCsLockWrapper mg(csLock, gpioIF, rtd->spiCookie, csTimeoutType, csTimeoutMs);
       if (mg.lockResult != returnvalue::OK or mg.gpioResult != returnvalue::OK) {
         sif::error << "Max31865RtdReader::periodicInitHandling: Manual CS lock failed" << std::endl;
@@ -95,13 +95,7 @@ bool Max31865RtdReader::periodicInitHandling() {
       if (result != returnvalue::OK) {
         handleSpiError(rtd, result, "clearFaultStatus");
       }
-      rtd->configured = true;
       rtd->db.configured = true;
-      if (rtd->active) {
-        rtd->db.active = true;
-      }
-    }
-    if (rtd->active and rtd->configured and not rtd->db.active) {
       rtd->db.active = true;
     }
   }
@@ -241,8 +235,8 @@ ReturnValue_t Max31865RtdReader::sendMessage(CookieIF* cookie, const uint8_t* se
         rtdCookie->cd.setTimeout(MAX31865::WARMUP_MS);
         rtdCookie->cd.resetTimer();
         rtdCookie->on = true;
-        rtdCookie->active = false;
+        rtdCookie->db.active = false;
-        rtdCookie->configured = false;
+        rtdCookie->db.configured = false;
         if (sendLen == 5) {
           thresholdHandler(rtdCookie, sendData);
         }
@@ -254,10 +248,10 @@ ReturnValue_t Max31865RtdReader::sendMessage(CookieIF* cookie, const uint8_t* se
         rtdCookie->cd.setTimeout(MAX31865::WARMUP_MS);
         rtdCookie->cd.resetTimer();
         rtdCookie->on = true;
-        rtdCookie->active = true;
+        rtdCookie->db.active = true;
-        rtdCookie->configured = false;
+        rtdCookie->db.configured = false;
       } else {
-        rtdCookie->active = true;
+        rtdCookie->db.active = true;
       }
       if (sendLen == 5) {
         thresholdHandler(rtdCookie, sendData);
@@ -266,8 +260,8 @@ ReturnValue_t Max31865RtdReader::sendMessage(CookieIF* cookie, const uint8_t* se
     }
     case (EiveMax31855::RtdCommands::OFF): {
       rtdCookie->on = false;
-      rtdCookie->active = false;
+      rtdCookie->db.active = false;
-      rtdCookie->configured = false;
+      rtdCookie->db.configured = false;
       break;
     }
     case (EiveMax31855::RtdCommands::HIGH_TRESHOLD): {

linux/devices/Max31865RtdLowlevelHandler.h

@@ -23,8 +23,6 @@ struct Max31865ReaderCookie : public CookieIF {
   Countdown cd = Countdown(MAX31865::WARMUP_MS);
 
   bool on = false;
-  bool configured = false;
-  bool active = false;
   bool writeLowThreshold = false;
   bool writeHighThreshold = false;
   uint16_t lowThreshold = 0;

linux/devices/startracker/StarTrackerHandler.cpp

@@ -240,13 +240,12 @@ void StarTrackerHandler::doStartUp() {
       // the device handler's submode to the star tracker's mode
       return;
     case StartupState::DONE:
-      submode = SUBMODE_BOOTLOADER;
       startupState = StartupState::IDLE;
       break;
     default:
       return;
   }
-  setMode(_MODE_TO_ON);
+  setMode(_MODE_TO_ON, SUBMODE_BOOTLOADER);
 }
 
 void StarTrackerHandler::doShutDown() {
@@ -654,7 +653,7 @@ void StarTrackerHandler::fillCommandAndReplyMap() {
 }
 
 ReturnValue_t StarTrackerHandler::isModeCombinationValid(Mode_t mode, Submode_t submode) {
-  if (this->mode == MODE_NORMAL && mode == MODE_ON) {
+  if (getMode() == MODE_NORMAL && mode == MODE_ON) {
     return TRANS_NOT_ALLOWED;
   }
   switch (mode) {
@@ -678,7 +677,7 @@ ReturnValue_t StarTrackerHandler::isModeCombinationValid(Mode_t mode, Submode_t
 }
 
 void StarTrackerHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) {
-  switch (mode) {
+  switch (getMode()) {
     case _MODE_TO_ON:
       doOnTransition(subModeFrom);
       break;
@@ -698,17 +697,18 @@ void StarTrackerHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) {
 }
 
 void StarTrackerHandler::doOnTransition(Submode_t subModeFrom) {
-  if (submode == SUBMODE_BOOTLOADER && subModeFrom == SUBMODE_FIRMWARE) {
+  uint8_t dhbSubmode = getSubmode();
+  if (dhbSubmode == SUBMODE_BOOTLOADER && subModeFrom == SUBMODE_FIRMWARE) {
     bootBootloader();
-  } else if (submode == SUBMODE_FIRMWARE && subModeFrom == SUBMODE_FIRMWARE) {
+  } else if (dhbSubmode == SUBMODE_FIRMWARE && subModeFrom == SUBMODE_FIRMWARE) {
     setMode(MODE_ON);
-  } else if (submode == SUBMODE_FIRMWARE && subModeFrom == SUBMODE_BOOTLOADER) {
+  } else if (dhbSubmode == SUBMODE_FIRMWARE && subModeFrom == SUBMODE_BOOTLOADER) {
     bootFirmware(MODE_ON);
-  } else if (submode == SUBMODE_BOOTLOADER && subModeFrom == SUBMODE_BOOTLOADER) {
+  } else if (dhbSubmode == SUBMODE_BOOTLOADER && subModeFrom == SUBMODE_BOOTLOADER) {
     setMode(MODE_ON);
-  } else if (submode == SUBMODE_BOOTLOADER && subModeFrom == SUBMODE_NONE) {
+  } else if (dhbSubmode == SUBMODE_BOOTLOADER && subModeFrom == SUBMODE_NONE) {
     setMode(MODE_ON);
-  } else if (submode == SUBMODE_FIRMWARE && subModeFrom == SUBMODE_NONE) {
+  } else if (dhbSubmode == SUBMODE_FIRMWARE && subModeFrom == SUBMODE_NONE) {
     setMode(MODE_ON);
   }
 }
@@ -2067,13 +2067,13 @@ ReturnValue_t StarTrackerHandler::checkCommand(ActionId_t actionId) {
     case startracker::REQ_SUBSCRIPTION:
     case startracker::REQ_LOG_SUBSCRIPTION:
     case startracker::REQ_DEBUG_CAMERA:
-      if (not(mode == MODE_ON && submode == startracker::Program::FIRMWARE)) {
+      if (not(getMode() == MODE_ON && getSubmode() == startracker::Program::FIRMWARE)) {
         return STARTRACKER_RUNNING_BOOTLOADER;
       }
       break;
     case startracker::FIRMWARE_UPDATE:
     case startracker::FLASH_READ:
-      if (not(mode == MODE_ON && submode == startracker::Program::BOOTLOADER)) {
+      if (not(getMode() == MODE_ON && getSubmode() == startracker::Program::BOOTLOADER)) {
         return STARTRACKER_RUNNING_FIRMWARE;
       }
       break;

linux/ipcore/PdecHandler.cpp

@@ -135,7 +135,6 @@ ReturnValue_t PdecHandler::polledOperation() {
 ReturnValue_t PdecHandler::irqOperation() {
   ReturnValue_t result = returnvalue::OK;
   int fd = open(uioNames.irq, O_RDWR);
-  sif::info << uioNames.irq << std::endl;
   if (fd < 0) {
     sif::error << "PdecHandler::irqOperation: Opening UIO IRQ file" << uioNames.irq << " failed"
                << std::endl;

mission/core/GenericFactory.cpp

@@ -86,8 +86,8 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun
   }
 
   {
-    PoolManager::LocalPoolConfig poolCfg = {{300, 16},  {300, 32},   {250, 64},
+    PoolManager::LocalPoolConfig poolCfg = {{300, 16},  {350, 32},   {350, 64},
-                                            {150, 128}, {120, 1024}, {120, 2048}};
+                                            {200, 128}, {150, 1024}, {150, 2048}};
     tmStore = new PoolManager(objects::TM_STORE, poolCfg);
   }
 
@@ -114,15 +114,15 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun
   tcpServer->enableWiretapping(true);
 #endif /* OBSW_TCP_SERVER_WIRETAPPING == 1 */
 #endif /* OBSW_USE_TMTC_TCP_BRIDGE == 0 */
-  tcpIpTmtcBridge->setMaxNumberOfPacketsStored(300);
+  tcpIpTmtcBridge->setMaxNumberOfPacketsStored(150);
 #endif /* OBSW_ADD_TCPIP_BRIDGE == 1 */
 
   auto* ccsdsDistrib =
       new CcsdsDistributor(config::EIVE_PUS_APID, objects::CCSDS_PACKET_DISTRIBUTOR);
   new PusDistributor(config::EIVE_PUS_APID, objects::PUS_PACKET_DISTRIBUTOR, ccsdsDistrib);
 
-  *cfdpFunnel = new CfdpTmFunnel(objects::CFDP_TM_FUNNEL, config::EIVE_CFDP_APID, *tmStore);
+  *cfdpFunnel = new CfdpTmFunnel(objects::CFDP_TM_FUNNEL, config::EIVE_CFDP_APID, *tmStore, 50);
-  *pusFunnel = new PusTmFunnel(objects::PUS_TM_FUNNEL, *timeStamper, *tmStore);
+  *pusFunnel = new PusTmFunnel(objects::PUS_TM_FUNNEL, *timeStamper, *tmStore, 80);
 #if OBSW_ADD_TCPIP_BRIDGE == 1
   (*cfdpFunnel)->addDestination(*tcpIpTmtcBridge, 0);
   (*pusFunnel)->addDestination(*tcpIpTmtcBridge, 0);

mission/devices/BpxBatteryHandler.cpp

@@ -216,7 +216,7 @@ ReturnValue_t BpxBatteryHandler::interpretDeviceReply(DeviceCommandId_t id, cons
       if (packet[2] != sentPingByte) {
         return DeviceHandlerIF::INVALID_DATA;
       }
-      if (mode == _MODE_START_UP) {
+      if (getMode() == _MODE_START_UP) {
         commandExecuted = true;
       }
       break;

mission/devices/GomspaceDeviceHandler.cpp

@@ -570,7 +570,7 @@ ReturnValue_t GomspaceDeviceHandler::generateRequestFullCfgTableCmd(DeviceType d
 
 uint32_t GomspaceDeviceHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 0; }
 
-void GomspaceDeviceHandler::setModeNormal() { mode = MODE_NORMAL; }
+void GomspaceDeviceHandler::setModeNormal() { setMode(_MODE_TO_NORMAL); }
 
 ReturnValue_t GomspaceDeviceHandler::printStatus(DeviceCommandId_t cmd) {
   sif::info << "No printHkTable implementation given.." << std::endl;

mission/devices/Max31865EiveHandler.cpp

@@ -30,16 +30,13 @@ void Max31865EiveHandler::doStartUp() {
 }
 
 void Max31865EiveHandler::doShutDown() {
-  updatePeriodicReply(false, EiveMax31855::RtdCommands::EXCHANGE_SET_ID);
   if (state == InternalState::NONE or state == InternalState::ACTIVE or
       state == InternalState::ON) {
     state = InternalState::INACTIVE;
     transitionOk = false;
-  } else {
-    transitionOk = true;
   }
   if (state == InternalState::INACTIVE and transitionOk) {
-    setMode(_MODE_POWER_DOWN);
+    setMode(MODE_OFF);
   }
 }
 
@@ -104,7 +101,7 @@ void Max31865EiveHandler::simpleCommand(EiveMax31855::RtdCommands cmd) {
   rawPacketLen = 1;
 }
 void Max31865EiveHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) {
-  if (mode == _MODE_TO_NORMAL) {
+  if (getMode() == _MODE_TO_NORMAL) {
     if (state != InternalState::ACTIVE) {
       state = InternalState::ACTIVE;
       transitionOk = false;
@@ -125,7 +122,7 @@ void Max31865EiveHandler::fillCommandAndReplyMap() {
 
 ReturnValue_t Max31865EiveHandler::scanForReply(const uint8_t* start, size_t remainingSize,
                                                 DeviceCommandId_t* foundId, size_t* foundLen) {
-  if (mode == _MODE_POWER_ON or mode == _MODE_WAIT_ON) {
+  if (getMode() == _MODE_POWER_ON or getMode() == _MODE_WAIT_ON) {
     return IGNORE_FULL_PACKET;
   }
   if (remainingSize != structLen) {
@@ -145,12 +142,17 @@ ReturnValue_t Max31865EiveHandler::interpretDeviceReply(DeviceCommandId_t id,
   if (result != returnvalue::OK) {
     return result;
   }
-  if (mode == _MODE_TO_NORMAL and exchangeStruct.active and state == InternalState::ACTIVE) {
+  if (getMode() == _MODE_TO_NORMAL and exchangeStruct.active and state == InternalState::ACTIVE) {
     transitionOk = true;
   }
-  if (mode == _MODE_START_UP and exchangeStruct.configured and state == InternalState::ON) {
+  if (getMode() == _MODE_START_UP and exchangeStruct.configured and state == InternalState::ON) {
     transitionOk = true;
   }
+  if (getMode() == _MODE_SHUT_DOWN and not exchangeStruct.active) {
+    transitionOk = true;
+    return returnvalue::OK;
+  }
+
   // Calculate resistance
   float rtdValue = exchangeStruct.adcCode * EiveMax31855::RTD_RREF_PT1000 / INT16_MAX;
   // calculate approximation

mission/devices/Max31865PT1000Handler.cpp

@@ -302,7 +302,7 @@ ReturnValue_t Max31865PT1000Handler::scanForReply(const uint8_t *start, size_t r
     } else if (internalState == InternalState::CLEAR_FAULT_BYTE) {
       *foundId = MAX31865::CLEAR_FAULT_BYTE;
       *foundLen = 2;
-      if (mode == _MODE_START_UP) {
+      if (getMode() == _MODE_START_UP) {
         commandExecuted = true;
       } else {
         internalState = InternalState::RUNNING;
@@ -524,7 +524,7 @@ void Max31865PT1000Handler::setInstantNormal(bool instantNormal) {
 }
 
 void Max31865PT1000Handler::modeChanged() {
-  if (mode == MODE_OFF) {
+  if (getMode() == MODE_OFF) {
     lastFaultStatus = 0;
     currentFaultStatus = 0;
     sameFaultStatusCounter = 0;

mission/devices/PayloadPcduHandler.cpp

@@ -66,7 +66,7 @@ void PayloadPcduHandler::doShutDown() {
 }
 
 void PayloadPcduHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) {
-  if (mode == _MODE_TO_NORMAL) {
+  if (getMode() == _MODE_TO_NORMAL) {
     stateMachineToNormal(modeFrom, subModeFrom);
     return;
   }
@@ -76,7 +76,7 @@ void PayloadPcduHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) {
 ReturnValue_t PayloadPcduHandler::stateMachineToNormal(Mode_t modeFrom, Submode_t subModeFrom) {
   using namespace plpcdu;
   bool doFinish = true;
-  if (((submode >> SOLID_STATE_RELAYS_ADC_ON) & 0b1) == 1) {
+  if (((getSubmode() >> SOLID_STATE_RELAYS_ADC_ON) & 0b1) == 1) {
     if (state == States::PL_PCDU_OFF) {
       sif::error << "PayloadPcduHandler::stateMachineToNormal: Unexpected state PL_PCDU_OFF"
                  << "detected" << std::endl;
@@ -129,7 +129,7 @@ ReturnValue_t PayloadPcduHandler::stateMachineToNormal(Mode_t modeFrom, Submode_
 
   auto switchHandler = [&](NormalSubmodeBits bit, gpioId_t id, std::string info) {
     if (((diffMask >> bit) & 1) == 1) {
-      if (((submode >> bit) & 1) == 1) {
+      if (((getSubmode() >> bit) & 1) == 1) {
 #if OBSW_VERBOSE_LEVEL >= 1
         sif::info << "Enabling PL PCDU " << info << " module" << std::endl;
 #endif
@@ -150,7 +150,7 @@ ReturnValue_t PayloadPcduHandler::stateMachineToNormal(Mode_t modeFrom, Submode_
   switchHandler(MPA_ON, gpioIds::PLPCDU_ENB_MPA, "MPA");
   switchHandler(HPA_ON, gpioIds::PLPCDU_ENB_HPA, "HPA");
   if (doFinish) {
-    setMode(MODE_NORMAL, submode);
+    setMode(MODE_NORMAL);
   }
   return returnvalue::OK;
 }
@@ -177,7 +177,7 @@ ReturnValue_t PayloadPcduHandler::buildTransitionDeviceCommand(DeviceCommandId_t
     *id = plpcdu::SETUP_CMD;
     return buildCommandFromCommand(*id, nullptr, 0);
   }
-  if (mode == _MODE_TO_NORMAL) {
+  if (getMode() == _MODE_TO_NORMAL) {
     return buildNormalDeviceCommand(id);
   }
   return NOTHING_TO_SEND;
@@ -248,7 +248,7 @@ ReturnValue_t PayloadPcduHandler::interpretDeviceReply(DeviceCommandId_t id,
   using namespace plpcdu;
   switch (id) {
     case (SETUP_CMD): {
-      if (mode == _MODE_TO_NORMAL) {
+      if (getMode() == _MODE_TO_NORMAL) {
         adcCmdExecuted = true;
       }
       break;
@@ -540,33 +540,34 @@ bool PayloadPcduHandler::checkCurrent(float val, float upperBound, Event event)
 ReturnValue_t PayloadPcduHandler::isModeCombinationValid(Mode_t mode, Submode_t submode) {
   using namespace plpcdu;
   if (mode == MODE_NORMAL) {
-    diffMask = submode ^ this->submode;
+    uint8_t dhbSubmode = getSubmode();
+    diffMask = submode ^ dhbSubmode;
     // Also deals with the case where the mode is MODE_ON, submode should be 0 here
     if ((((submode >> SOLID_STATE_RELAYS_ADC_ON) & 0b1) == SOLID_STATE_RELAYS_ADC_ON) and
-        (this->mode == MODE_NORMAL and this->submode != ALL_OFF_SUBMODE)) {
+        (getMode() == MODE_NORMAL and dhbSubmode != ALL_OFF_SUBMODE)) {
       return TRANS_NOT_ALLOWED;
     }
     if (((((submode >> DRO_ON) & 1) == 1) and
-         ((this->submode & 0b1) != (1 << SOLID_STATE_RELAYS_ADC_ON)))) {
+         ((dhbSubmode & 0b1) != (1 << SOLID_STATE_RELAYS_ADC_ON)))) {
       return TRANS_NOT_ALLOWED;
     }
     if ((((submode >> X8_ON) & 1) == 1) and
-        ((this->submode & 0b11) != ((1 << SOLID_STATE_RELAYS_ADC_ON) | (1 << DRO_ON)))) {
+        ((dhbSubmode & 0b11) != ((1 << SOLID_STATE_RELAYS_ADC_ON) | (1 << DRO_ON)))) {
       return TRANS_NOT_ALLOWED;
     }
     if (((((submode >> TX_ON) & 1) == 1) and
-         ((this->submode & 0b111) !=
+         ((dhbSubmode & 0b111) !=
           ((1 << X8_ON) | (1 << DRO_ON) | (1 << SOLID_STATE_RELAYS_ADC_ON))))) {
       return TRANS_NOT_ALLOWED;
     }
     if ((((submode >> MPA_ON) & 1) == 1 and
-         ((this->submode & 0b1111) !=
+         ((dhbSubmode & 0b1111) !=
           ((1 << TX_ON) | (1 << X8_ON) | (1 << DRO_ON) | (1 << SOLID_STATE_RELAYS_ADC_ON))))) {
       return TRANS_NOT_ALLOWED;
     }
     if ((((submode >> HPA_ON) & 1) == 1 and
-         ((this->submode & 0b11111) != ((1 << MPA_ON) | (1 << TX_ON) | (1 << X8_ON) |
+         ((dhbSubmode & 0b11111) != ((1 << MPA_ON) | (1 << TX_ON) | (1 << X8_ON) | (1 << DRO_ON) |
-                                        (1 << DRO_ON) | (1 << SOLID_STATE_RELAYS_ADC_ON))))) {
+                                     (1 << SOLID_STATE_RELAYS_ADC_ON))))) {
       return TRANS_NOT_ALLOWED;
     }
     return returnvalue::OK;

mission/devices/SusHandler.cpp

@@ -150,7 +150,7 @@ ReturnValue_t SusHandler::scanForReply(const uint8_t *start, size_t remainingSiz
 ReturnValue_t SusHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) {
   switch (id) {
     case SUS::WRITE_SETUP: {
-      if (mode == _MODE_START_UP) {
+      if (getMode() == _MODE_START_UP) {
         commandExecuted = true;
       }
       return returnvalue::OK;

mission/devices/SyrlinksHkHandler.cpp

@@ -630,7 +630,7 @@ ReturnValue_t SyrlinksHkHandler::initializeLocalDataPool(localpool::DataPool& lo
   return returnvalue::OK;
 }
 
-void SyrlinksHkHandler::setModeNormal() { mode = MODE_NORMAL; }
+void SyrlinksHkHandler::setModeNormal() { setMode(MODE_NORMAL); }
 
 float SyrlinksHkHandler::calcTempVal(uint16_t raw) { return 0.126984 * raw - 67.87; }
 

mission/devices/Tmp1075Handler.cpp

@@ -12,7 +12,7 @@ Tmp1075Handler::Tmp1075Handler(object_id_t objectId, object_id_t comIF, CookieIF
 Tmp1075Handler::~Tmp1075Handler() {}
 
 void Tmp1075Handler::doStartUp() {
-  if (mode == _MODE_START_UP) {
+  if (getMode() == _MODE_START_UP) {
     setMode(MODE_ON);
   }
 }

mission/tmtc/CfdpTmFunnel.cpp

@@ -5,8 +5,8 @@
 #include "fsfw/tmtcservices/TmTcMessage.h"
 
 CfdpTmFunnel::CfdpTmFunnel(object_id_t objectId, uint16_t cfdpInCcsdsApid,
-                           StorageManagerIF& tmStore)
+                           StorageManagerIF& tmStore, uint32_t messageDepth)
-    : TmFunnelBase(objectId, tmStore, 10), cfdpInCcsdsApid(cfdpInCcsdsApid) {}
+    : TmFunnelBase(objectId, tmStore, messageDepth), cfdpInCcsdsApid(cfdpInCcsdsApid) {}
 
 const char* CfdpTmFunnel::getName() const { return "CFDP TM Funnel"; }
 
@@ -70,13 +70,20 @@ ReturnValue_t CfdpTmFunnel::handlePacket(TmTcMessage& msg) {
   for (unsigned int idx = 0; idx < destinations.size(); idx++) {
     const auto& destVcidPair = destinations[idx];
     if (destinations.size() > 1) {
-      if (idx <= destinations.size() - 1) {
+      if (idx < destinations.size() - 1) {
         // Create copy of data to ensure each TM recipient has its own copy. That way, we don't need
         // to bother with send order and where the data is deleted.
         store_address_t storeId;
         result = tmStore.addData(&storeId, newPacketData, serSize);
+        if (result == returnvalue::OK) {
           msg.setStorageId(storeId);
         } else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+          sif::error << "PusTmFunnel::handlePacket: Store too full to create data copy"
+                     << std::endl;
+#endif
+        }
+      } else {
         msg.setStorageId(origStoreId);
       }
     }

mission/tmtc/CfdpTmFunnel.h

@@ -12,7 +12,8 @@
 
 class CfdpTmFunnel : public TmFunnelBase {
  public:
-  CfdpTmFunnel(object_id_t objectId, uint16_t cfdpInCcsdsApid, StorageManagerIF& tmStore);
+  CfdpTmFunnel(object_id_t objectId, uint16_t cfdpInCcsdsApid, StorageManagerIF& tmStore,
+               uint32_t messageDepth);
   [[nodiscard]] const char* getName() const override;
   ReturnValue_t performOperation(uint8_t opCode);
   ReturnValue_t initialize() override;

mission/tmtc/PusTmFunnel.cpp

@@ -51,13 +51,20 @@ ReturnValue_t PusTmFunnel::handlePacket(TmTcMessage &message) {
   for (unsigned int idx = 0; idx < destinations.size(); idx++) {
     const auto &destVcidPair = destinations[idx];
     if (destinations.size() > 1) {
-      if (idx <= destinations.size() - 1) {
+      if (idx < destinations.size() - 1) {
         // Create copy of data to ensure each TM recipient has its own copy. That way, we don't need
         // to bother with send order and where the data is deleted.
         store_address_t storeId;
         result = tmStore.addData(&storeId, packetData, size);
+        if (result == returnvalue::OK) {
           message.setStorageId(storeId);
         } else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+          sif::error << "PusTmFunnel::handlePacket: Store too full to create data copy"
+                     << std::endl;
+#endif
+        }
+      } else {
         message.setStorageId(origStoreId);
       }
     }

mission/tmtc/VirtualChannel.cpp

@@ -34,7 +34,7 @@ ReturnValue_t VirtualChannel::performOperation() {
     size_t size = 0;
     result = tmStore->getData(storeId, &data, &size);
     if (result != returnvalue::OK) {
-      sif::warning << "VirtualChannel::performOperation: Failed to read data from IPC store"
+      sif::warning << "VirtualChannel::performOperation: Failed to read data from TM store"
                    << std::endl;
       tmStore->deleteData(storeId);
       return result;