changelog, prep v6.0.0

Reviewed-on: #724

CHANGELOG.md

@@ -16,11 +16,57 @@ will consitute of a breaking change warranting a new major release:
 
 # [unreleased]
 
-# [v6.0.0] to be released
+# [v6.0.0] 2023-07-02
 
+- `q7s-package` version v3.2.0
 - Important bugfixes for PTME. See `q7s-package` CHANGELOG.
 
-# [v5.1.0] to be released
+## Changed
+
+- Added back PTME busy bit polling. This is necessary due to changes to the AXI APB interface
+  to the PTME core.
+
+## Fixed
+
+- For the live channel (VC0), telemetry was still only dumped if the transmitter is active.
+  Please note that this fix will lead to crashes for FW versions below v3.2.
+  However, it might not be an issue for the oldest firmware on the satellite (v2.5.1).
+
+# [v5.2.0] 2023-07-02
+
+## Fixed
+
+- The firmware information event was not triggered even when possible because of an ordering
+  bug in the initializer function.
+- Empty dumps (no TM in time range) will now correctly be completed immediately
+
+## Changed
+
+- PTME was always reset on submode changes. The reset will now only be performed if the actual data
+  rate changes.
+- Add back ACS board code for the EM. Now that the radiation sensor was removed, the image switching
+  issue has disappeared and adding back the ACS board is worth it for the GPS timekeeping.
+
+# [v5.1.0] 2023-06-28
+
+- `eive-tmtc` version v5.1.0
+
+## Changed
+
+- Persistent TM store dumps are now performed in chronological order.
+- Increase Syrlinks RX HK rate to 5.0 seconds during a pass.
+- Various robustness improvements for the heater handler. The heater handler will now only
+  process the command queue if it is not busy with switch commanding which reduces the amount
+  of possible bugs.
+- The heater handler is only able to process messages stricly sequentially now but is scheduled
+  twice in a 0.5 second slot so something like a consecutive heater ON or OFF command can still
+  be handled relatively quickly.
+
+## Added
+
+- Sequence counters for PUS and CFDP packets are now stored persistently across graceful reboots.
+- The PUS packet message type counter will now be incremented properly for each PUS service.
+- Internal error reporter set is now enabled by default and generated every 120 seconds.
 
 # [v5.0.0] 2023-06-26
 

CMakeLists.txt

@@ -9,7 +9,7 @@
 # ##############################################################################
 cmake_minimum_required(VERSION 3.13)
 
-set(OBSW_VERSION_MAJOR 5)
+set(OBSW_VERSION_MAJOR 6)
 set(OBSW_VERSION_MINOR 0)
 set(OBSW_VERSION_REVISION 0)
 
@@ -105,7 +105,7 @@ set(OBSW_ADD_THERMAL_TEMP_INSERTER
     ${OBSW_Q7S_EM}
     CACHE STRING "Add thermal sensor temperature inserter")
 set(OBSW_ADD_ACS_BOARD
-    ${INIT_VAL}
+    1
     CACHE STRING "Add ACS board module")
 set(OBSW_ADD_GPS_CTRL
     ${INIT_VAL}

README.md

@@ -964,6 +964,12 @@ used by other software components to read the current chip and copy.
 This is a configuration scripts which runs after the Network Time Protocol has run. This script
 currently sets the static IP address `192.168.133.10` and starts the `can` interface.
 
+## Initial boot delay
+
+You can create a file named `boot_delays_secs.txt` inside the home folder to delay the OBSW boot
+for 6 seconds if the file is empty of for the number of seconds specified inside the file. This
+can be helpful if something inside the OBSW leads to an immediate crash of the OBC.
+
 ## PCDU
 
 Connect to serial console of P60 Dock

bsp_hosted/ObjectFactory.cpp

@@ -68,7 +68,7 @@ void ObjectFactory::produce(void* args) {
 #endif
   auto sdcMan = new DummySdCardManager("/tmp");
   ObjectFactory::produceGenericObjects(nullptr, &pusFunnel, &cfdpFunnel, *sdcMan, &ipcStore,
-                                       &tmStore, persistentStores, 120);
+                                       &tmStore, persistentStores, 120, enableHkSets);
 
   new TmFunnelHandler(objects::LIVE_TM_TASK, *pusFunnel, *cfdpFunnel);
   auto* dummyGpioIF = new DummyGpioIF();

bsp_q7s/boardconfig/busConf.h

@@ -85,13 +85,9 @@ static constexpr char EN_RW_4[] = "enable_rw_4";
 static constexpr char RAD_SENSOR_CHIP_SELECT[] = "rad_sensor_chip_select";
 static constexpr char ENABLE_RADFET[] = "enable_radfet";
 
-static constexpr char PAPB_BUSY_SIGNAL_VC0[] = "papb_busy_signal_vc0";
 static constexpr char PAPB_EMPTY_SIGNAL_VC0[] = "papb_empty_signal_vc0";
-static constexpr char PAPB_BUSY_SIGNAL_VC1[] = "papb_busy_signal_vc1";
 static constexpr char PAPB_EMPTY_SIGNAL_VC1[] = "papb_empty_signal_vc1";
-static constexpr char PAPB_BUSY_SIGNAL_VC2[] = "papb_busy_signal_vc2";
 static constexpr char PAPB_EMPTY_SIGNAL_VC2[] = "papb_empty_signal_vc2";
-static constexpr char PAPB_BUSY_SIGNAL_VC3[] = "papb_busy_signal_vc3";
 static constexpr char PAPB_EMPTY_SIGNAL_VC3[] = "papb_empty_signal_vc3";
 
 static constexpr char PTME_RESETN[] = "ptme_resetn";

bsp_q7s/core/CoreController.cpp

@@ -170,9 +170,6 @@ ReturnValue_t CoreController::initialize() {
 
   sdStateMachine();
 
-  triggerEvent(core::REBOOT_SW, CURRENT_CHIP, CURRENT_COPY);
-  announceCurrentImageInfo();
-  announceVersionInfo();
   EventManagerIF *eventManager =
       ObjectManager::instance()->get<EventManagerIF>(objects::EVENT_MANAGER);
   if (eventManager == nullptr or eventQueue == nullptr) {
@@ -189,17 +186,22 @@ ReturnValue_t CoreController::initialize() {
   if (result != returnvalue::OK) {
     sif::warning << "Subscribing for GPS GPS_FIX_CHANGE event failed" << std::endl;
   }
-
+  triggerEvent(core::REBOOT_SW, CURRENT_CHIP, CURRENT_COPY);
+  announceCurrentImageInfo();
+  // This has to come before the version announce because it might be required for retrieving
+  // the firmware version.
   if (common::OBSW_VERSION_MAJOR >= 6 or common::OBSW_VERSION_MAJOR == 4) {
     UioMapper sysRomMapper(q7s::UIO_SYS_ROM);
     result = sysRomMapper.getMappedAdress(&mappedSysRomAddr, UioMapper::Permissions::READ_ONLY);
     if (result != returnvalue::OK) {
       // TODO: This might be a reason to switch to another image..
       sif::error << "Getting mapped SYS ROM UIO address failed" << std::endl;
-      return ObjectManager::CHILD_INIT_FAILED;
+      result = ObjectManager::CHILD_INIT_FAILED;
     }
   }
-  return returnvalue::OK;
+  announceVersionInfo();
+
+  return result;
 }
 
 ReturnValue_t CoreController::initializeAfterTaskCreation() {
@@ -1224,6 +1226,10 @@ ReturnValue_t CoreController::actionReboot(const uint8_t *data, size_t size) {
 
 ReturnValue_t CoreController::gracefulShutdownTasks(xsc::Chip chip, xsc::Copy copy,
                                                     bool &protOpPerformed) {
+  // Store both sequence counters persistently.
+  core::SAVE_CFDP_SEQUENCE_COUNT = true;
+  core::SAVE_PUS_SEQUENCE_COUNT = true;
+
   sdcMan->setBlocking(true);
   sdcMan->markUnusable();
   // Wait two seconds to ensure no one uses the SD cards

bsp_q7s/core/ObjectFactory.cpp

@@ -731,20 +731,12 @@ ReturnValue_t ObjectFactory::createCcsdsComponents(CcsdsComponentArgs& args) {
   // GPIO definitions of signals connected to the virtual channel interfaces of the PTME IP Core
   GpioCookie* gpioCookiePtmeIp = new GpioCookie;
   GpiodRegularByLineName* gpio = nullptr;
-  gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_BUSY_SIGNAL_VC0, "PAPB VC0");
-  gpioCookiePtmeIp->addGpio(gpioIds::VC0_PAPB_BUSY, gpio);
   gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_EMPTY_SIGNAL_VC0, "PAPB VC0");
   gpioCookiePtmeIp->addGpio(gpioIds::VC0_PAPB_EMPTY, gpio);
-  gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_BUSY_SIGNAL_VC1, "PAPB VC1");
-  gpioCookiePtmeIp->addGpio(gpioIds::VC1_PAPB_BUSY, gpio);
   gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_EMPTY_SIGNAL_VC1, "PAPB VC1");
   gpioCookiePtmeIp->addGpio(gpioIds::VC1_PAPB_EMPTY, gpio);
-  gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_BUSY_SIGNAL_VC2, "PAPB VC2");
-  gpioCookiePtmeIp->addGpio(gpioIds::VC2_PAPB_BUSY, gpio);
   gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_EMPTY_SIGNAL_VC2, "PAPB VC2");
   gpioCookiePtmeIp->addGpio(gpioIds::VC2_PAPB_EMPTY, gpio);
-  gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_BUSY_SIGNAL_VC3, "PAPB VC3");
-  gpioCookiePtmeIp->addGpio(gpioIds::VC3_PAPB_BUSY, gpio);
   gpio = new GpiodRegularByLineName(q7s::gpioNames::PAPB_EMPTY_SIGNAL_VC3, "PAPB VC3");
   gpioCookiePtmeIp->addGpio(gpioIds::VC3_PAPB_EMPTY, gpio);
   gpio = new GpiodRegularByLineName(q7s::gpioNames::PTME_RESETN, "PTME RESETN",
@@ -753,19 +745,14 @@ ReturnValue_t ObjectFactory::createCcsdsComponents(CcsdsComponentArgs& args) {
   gpioChecker(args.gpioComIF.addGpios(gpioCookiePtmeIp), "PTME PAPB VCs");
 
   // Creating virtual channel interfaces
-  VirtualChannelIF* vc0 =
-      new PapbVcInterface(&args.gpioComIF, gpioIds::VC0_PAPB_BUSY, gpioIds::VC0_PAPB_EMPTY,
-                          q7s::UIO_PTME, q7s::uiomapids::PTME_VC0);
-  VirtualChannelIF* vc1 =
-      new PapbVcInterface(&args.gpioComIF, gpioIds::VC1_PAPB_BUSY, gpioIds::VC1_PAPB_EMPTY,
-                          q7s::UIO_PTME, q7s::uiomapids::PTME_VC1);
-  VirtualChannelIF* vc2 =
-      new PapbVcInterface(&args.gpioComIF, gpioIds::VC2_PAPB_BUSY, gpioIds::VC2_PAPB_EMPTY,
-                          q7s::UIO_PTME, q7s::uiomapids::PTME_VC2);
-  VirtualChannelIF* vc3 =
-      new PapbVcInterface(&args.gpioComIF, gpioIds::VC3_PAPB_BUSY, gpioIds::VC3_PAPB_EMPTY,
-                          q7s::UIO_PTME, q7s::uiomapids::PTME_VC3);
-
+  VirtualChannelIF* vc0 = new PapbVcInterface(&args.gpioComIF, gpioIds::VC0_PAPB_EMPTY,
+                                              q7s::UIO_PTME, q7s::uiomapids::PTME_VC0);
+  VirtualChannelIF* vc1 = new PapbVcInterface(&args.gpioComIF, gpioIds::VC1_PAPB_EMPTY,
+                                              q7s::UIO_PTME, q7s::uiomapids::PTME_VC1);
+  VirtualChannelIF* vc2 = new PapbVcInterface(&args.gpioComIF, gpioIds::VC2_PAPB_EMPTY,
+                                              q7s::UIO_PTME, q7s::uiomapids::PTME_VC2);
+  VirtualChannelIF* vc3 = new PapbVcInterface(&args.gpioComIF, gpioIds::VC3_PAPB_EMPTY,
+                                              q7s::UIO_PTME, q7s::uiomapids::PTME_VC3);
   // Creating ptme object and adding virtual channel interfaces
   Ptme* ptme = new Ptme(objects::PTME);
   ptme->addVcInterface(ccsds::VC0, vc0);

bsp_q7s/core/scheduling.cpp

@@ -324,6 +324,10 @@ void scheduling::initTasks() {
   if (result != returnvalue::OK) {
     scheduling::printAddObjectError("HEATER_HANDLER", objects::HEATER_HANDLER);
   }
+  result = tcsSystemTask->addComponent(objects::HEATER_HANDLER);
+  if (result != returnvalue::OK) {
+    scheduling::printAddObjectError("HEATER_HANDLER", objects::HEATER_HANDLER);
+  }
 
 #if OBSW_ADD_SYRLINKS == 1
   PeriodicTaskIF* syrlinksCom = factory->createPeriodicTask(

bsp_q7s/em/emObjectFactory.cpp

@@ -36,8 +36,8 @@ void ObjectFactory::produce(void* args) {
 
   PersistentTmStores stores;
   ObjectFactory::produceGenericObjects(&healthTable, &pusFunnel, &cfdpFunnel,
-                                       *SdCardManager::instance(), &ipcStore, &tmStore, stores,
-                                       200);
+                                       *SdCardManager::instance(), &ipcStore, &tmStore, stores, 200,
+                                       enableHkSets);
 
   LinuxLibgpioIF* gpioComIF = nullptr;
   SerialComIF* uartComIF = nullptr;
@@ -84,9 +84,10 @@ void ObjectFactory::produce(void* args) {
 
   new CoreController(objects::CORE_CONTROLLER, enableHkSets);
 
-#if OBSW_ADD_ACS_BOARD == 1
-  // Still initialize chip select to avoid SPI bus issues.
+  // Initialize chip select to avoid SPI bus issues.
   createRadSensorChipSelect(gpioComIF);
+
+#if OBSW_ADD_ACS_BOARD == 1
   createAcsBoardComponents(*spiMainComIF, gpioComIF, uartComIF, *pwrSwitcher, true,
                            adis1650x::Type::ADIS16507);
 #else

bsp_q7s/fmObjectFactory.cpp

@@ -33,8 +33,8 @@ void ObjectFactory::produce(void* args) {
 
   PersistentTmStores stores;
   ObjectFactory::produceGenericObjects(&healthTable, &pusFunnel, &cfdpFunnel,
-                                       *SdCardManager::instance(), &ipcStore, &tmStore, stores,
-                                       200);
+                                       *SdCardManager::instance(), &ipcStore, &tmStore, stores, 200,
+                                       true);
 
   LinuxLibgpioIF* gpioComIF = nullptr;
   SerialComIF* uartComIF = nullptr;

common/config/devices/gpioIds.h

@@ -96,13 +96,9 @@ enum gpioId_t {
 
   SPI_MUX,
   VC0_PAPB_EMPTY,
-  VC0_PAPB_BUSY,
   VC1_PAPB_EMPTY,
-  VC1_PAPB_BUSY,
   VC2_PAPB_EMPTY,
-  VC2_PAPB_BUSY,
   VC3_PAPB_EMPTY,
-  VC3_PAPB_BUSY,
   PTME_RESETN,
 
   PDEC_RESET,

common/config/eive/definitions.h

@@ -11,6 +11,8 @@ static constexpr char SD_1_MOUNT_POINT[] = "/mnt/sd1";
 static constexpr char OBSW_UPDATE_ARCHIVE_FILE_NAME[] = "eive-sw-update.tar.xz";
 static constexpr char STRIPPED_OBSW_BINARY_FILE_NAME[] = "eive-obsw-stripped";
 static constexpr char OBSW_VERSION_FILE_NAME[] = "obsw_version.txt";
+static constexpr char PUS_SEQUENCE_COUNT_FILE[] = "pus-sequence-count.txt";
+static constexpr char CFDP_SEQUENCE_COUNT_FILE[] = "cfdp-sequence-count.txt";
 
 static constexpr char OBSW_PATH[] = "/usr/bin/eive-obsw";
 static constexpr char OBSW_VERSION_FILE_PATH[] = "/usr/share/eive-obsw/obsw_version.txt";

dummies/TemperatureSensorInserter.cpp

@@ -96,6 +96,25 @@ ReturnValue_t TemperatureSensorInserter::performOperation(uint8_t opCode) {
       }
       break;
     }
+    case (TestCase::COLD_PLOC_CONSECUTIVE): {
+      if (cycles == 15) {
+        sif::debug << "Setting cold PLOC temperature" << std::endl;
+        max31865DummyMap[objects::RTD_0_IC3_PLOC_HEATSPREADER]->setTemperature(-15, true);
+      }
+      if (cycles == 30) {
+        sif::debug << "Setting warmer PLOC temperature" << std::endl;
+        max31865DummyMap[objects::RTD_0_IC3_PLOC_HEATSPREADER]->setTemperature(0, true);
+      }
+      if (cycles == 45) {
+        sif::debug << "Setting cold PLOC temperature again" << std::endl;
+        max31865DummyMap[objects::RTD_0_IC3_PLOC_HEATSPREADER]->setTemperature(-15, true);
+      }
+      if (cycles == 60) {
+        sif::debug << "Setting warmer PLOC temperature again" << std::endl;
+        max31865DummyMap[objects::RTD_0_IC3_PLOC_HEATSPREADER]->setTemperature(0, true);
+      }
+      break;
+    }
     case (TestCase::COLD_CAMERA): {
       if (cycles == 15) {
         sif::debug << "Setting cold CAM temperature" << std::endl;

dummies/TemperatureSensorInserter.h

@@ -32,6 +32,7 @@ class TemperatureSensorInserter : public ExecutableObjectIF, public SystemObject
     COLD_STR = 4,
     COLD_STR_CONSECUTIVE = 5,
     COLD_CAMERA = 6,
+    COLD_PLOC_CONSECUTIVE = 7,
   };
   int iteration = 0;
   uint32_t cycles = 0;

linux/ipcore/AxiPtmeConfig.cpp

@@ -16,9 +16,9 @@ AxiPtmeConfig::AxiPtmeConfig(object_id_t objectId, std::string axiUio, int mapNu
 AxiPtmeConfig::~AxiPtmeConfig() {}
 
 ReturnValue_t AxiPtmeConfig::initialize() {
-  ReturnValue_t result = returnvalue::OK;
   UioMapper uioMapper(axiUio, mapNum);
-  result = uioMapper.getMappedAdress(&baseAddress, UioMapper::Permissions::READ_WRITE);
+  ReturnValue_t result =
+      uioMapper.getMappedAdress(&baseAddress, UioMapper::Permissions::READ_WRITE);
   if (result != returnvalue::OK) {
     return result;
   }
@@ -26,8 +26,7 @@ ReturnValue_t AxiPtmeConfig::initialize() {
 }
 
 ReturnValue_t AxiPtmeConfig::writeCaduRateReg(uint8_t rateVal) {
-  ReturnValue_t result = returnvalue::OK;
-  result = mutex->lockMutex(timeoutType, mutexTimeout);
+  ReturnValue_t result = mutex->lockMutex(timeoutType, mutexTimeout);
   if (result != returnvalue::OK) {
     sif::warning << "AxiPtmeConfig::writeCaduRateReg: Failed to lock mutex" << std::endl;
     return returnvalue::FAILED;
@@ -41,6 +40,11 @@ ReturnValue_t AxiPtmeConfig::writeCaduRateReg(uint8_t rateVal) {
   return returnvalue::OK;
 }
 
+uint8_t AxiPtmeConfig::readCaduRateReg() {
+  MutexGuard mg(mutex);
+  return static_cast<uint8_t>(*(baseAddress + CADU_BITRATE_REG));
+}
+
 void AxiPtmeConfig::enableTxclockManipulator() {
   writeBit(COMMON_CONFIG_REG, true, BitPos::EN_TX_CLK_MANIPULATOR);
 }

linux/ipcore/AxiPtmeConfig.h

@@ -38,6 +38,7 @@ class AxiPtmeConfig : public SystemObject {
    *        frequency of the clock connected to the bit clock input of PTME.
    */
   ReturnValue_t writeCaduRateReg(uint8_t rateVal);
+  uint8_t readCaduRateReg();
 
   /**
    * @brief Next to functions control the tx clock manipulator component

linux/ipcore/PapbVcInterface.cpp

@@ -7,20 +7,16 @@
 
 #include "fsfw/serviceinterface/ServiceInterface.h"
 
-PapbVcInterface::PapbVcInterface(LinuxLibgpioIF* gpioComIF, gpioId_t papbBusyId,
-                                 gpioId_t papbEmptyId, std::string uioFile, int mapNum)
-    : gpioComIF(gpioComIF),
-      papbBusyId(papbBusyId),
-      papbEmptyId(papbEmptyId),
-      uioFile(std::move(uioFile)),
-      mapNum(mapNum) {}
+PapbVcInterface::PapbVcInterface(LinuxLibgpioIF* gpioComIF, gpioId_t papbEmptyId,
+                                 std::string uioFile, int mapNum)
+    : gpioComIF(gpioComIF), papbEmptyId(papbEmptyId), uioFile(std::move(uioFile)), mapNum(mapNum) {}
 
 PapbVcInterface::~PapbVcInterface() {}
 
 ReturnValue_t PapbVcInterface::initialize() {
   UioMapper uioMapper(uioFile, mapNum);
   ReturnValue_t result = uioMapper.getMappedAdress(const_cast<uint32_t**>(&vcBaseReg),
-                                                   UioMapper::Permissions::WRITE_ONLY);
+                                                   UioMapper::Permissions::READ_WRITE);
   if (result != returnvalue::OK) {
     return result;
   }
@@ -32,63 +28,27 @@ ReturnValue_t PapbVcInterface::write(const uint8_t* data, size_t size) {
   if (size < 4) {
     return returnvalue::FAILED;
   }
-  if (pollInterfaceReadiness(0, true) == returnvalue::OK) {
+  if (pollReadyForPacket()) {
     startPacketTransfer(ByteWidthCfg::ONE);
   } else {
     return DirectTmSinkIF::IS_BUSY;
   }
-  // TODO: This should work but does not.. :(
-  //  size_t idx = 0;
-  //  while (idx < size) {
-  //
-  //    nanosleep(&BETWEEN_POLL_DELAY, &remDelay);
-  //    if ((size - idx) < 4) {
-  //      *vcBaseReg = CONFIG_DATA_INPUT | (size - idx - 1);
-  //      usleep(1);
-  //    }
-  //    if (pollPapbBusySignal(2) == returnvalue::OK) {
-  //      //      vcBaseReg + DATA_REG_OFFSET + 3 = static_cast<uint8_t>(data + idx);
-  //      //      vcBaseReg + DATA_REG_OFFSET + 2 = static_cast<uint8_t>(data + idx + 1);
-  //      //      vcBaseReg + DATA_REG_OFFSET + 1 = static_cast<uint8_t>(data + idx + 2);
-  //      //      vcBaseReg + DATA_REG_OFFSET = static_cast<uint8_t>(data + idx + 3);
-  //
-  //      // std::memcpy((vcBaseReg + DATA_REG_OFFSET), data + idx , nextWriteSize);
-  //      *(vcBaseReg + DATA_REG_OFFSET) = *reinterpret_cast<const uint32_t*>(data + idx);
-  //      //uint8_t* byteReg = reinterpret_cast<uint8_t*>(vcBaseReg + DATA_REG_OFFSET);
-  //
-  //      //byteReg[0] = data[idx];
-  //      //byteReg[1] = data[idx];
-  //    } else {
-  //      abortPacketTransfer();
-  //      return returnvalue::FAILED;
-  //    }
-  //    // TODO: Change this after the bugfix. Right now, the PAPB ignores the content of the byte
-  //    // width configuration.5
-  //    // It's okay to increment by a larger amount for the last segment here, loop will be over
-  //    // in any case.
-  //    idx += 4;
-  //  }
-  for (size_t idx = 0; idx < size; idx++) {
-    // This delay is super-important, DO NOT REMOVE!
-    // Polling the GPIO or the config register too often messes up the scheduler.
-    // TODO: Maybe this should not be done like this. It would be better if there was a custom
-    // FPGA module which can accept packets and then takes care of dumping that packet into
-    // the PTME. DMA would be an ideal solution for this.
-    nanosleep(&BETWEEN_POLL_DELAY, &remDelay);
-    if (pollInterfaceReadiness(2, false) == returnvalue::OK) {
-      *(vcBaseReg + DATA_REG_OFFSET) = static_cast<uint32_t>(data[idx]);
-    } else {
-      abortPacketTransfer();
-      return returnvalue::FAILED;
-    }
-  }
-  nanosleep(&BETWEEN_POLL_DELAY, &remDelay);
-  if (pollInterfaceReadiness(2, false) == returnvalue::OK) {
-    completePacketTransfer();
-  } else {
+  if (not pollReadyForOctet(MAX_BUSY_POLLS)) {
     abortPacketTransfer();
     return returnvalue::FAILED;
   }
+  for (size_t idx = 0; idx < size; idx++) {
+    if (not pollReadyForOctet(MAX_BUSY_POLLS)) {
+      abortPacketTransfer();
+      return returnvalue::FAILED;
+    }
+    *(vcBaseReg + DATA_REG_OFFSET) = static_cast<uint32_t>(data[idx]);
+  }
+  if (not pollReadyForOctet(MAX_BUSY_POLLS)) {
+    abortPacketTransfer();
+    return returnvalue::FAILED;
+  }
+  completePacketTransfer();
   return returnvalue::OK;
 }
 
@@ -98,63 +58,49 @@ void PapbVcInterface::startPacketTransfer(ByteWidthCfg initWidth) {
 
 void PapbVcInterface::completePacketTransfer() { *vcBaseReg = CONFIG_END; }
 
-ReturnValue_t PapbVcInterface::pollInterfaceReadiness(uint32_t maxPollRetries,
-                                                      bool checkReadyState) const {
-  uint32_t busyIdx = 0;
-  nextDelay.tv_nsec = FIRST_DELAY_PAPB_POLLING_NS;
-
-  while (true) {
-    // Check if PAPB interface is ready to receive data. Use the configuration register for this.
-    // Bit 5, see PTME ptme_001_01-0-7-r2 Table 31.
-    uint32_t reg = *vcBaseReg;
-    bool busy = (reg >> 5) & 0b1;
-    bool ready = (reg >> 6) & 0b1;
-    if (not busy) {
-      return returnvalue::OK;
-    }
-    if (checkReadyState and not ready) {
-      return PAPB_BUSY;
-    }
-
-    busyIdx++;
-    if (busyIdx >= maxPollRetries) {
-      return PAPB_BUSY;
-    }
-
-    // Ignore signal handling here for now.
-    nanosleep(&nextDelay, &remDelay);
-    // Adaptive delay.
-    if (nextDelay.tv_nsec * 2 <= MAX_DELAY_PAPB_POLLING_NS) {
-      nextDelay.tv_nsec *= 2;
-    }
-  }
-  return returnvalue::OK;
+bool PapbVcInterface::pollReadyForPacket() const {
+  //  Check if PAPB interface is ready to receive data. Use the configuration register for this.
+  //  Bit 5, see PTME ptme_001_01-0-7-r2 Table 31.
+  uint32_t reg = *vcBaseReg;
+  return (reg >> 6) & 0b1;
 }
 
-void PapbVcInterface::isVcInterfaceBufferEmpty() {
+bool PapbVcInterface::isVcInterfaceBufferEmpty() {
   ReturnValue_t result = returnvalue::OK;
   gpio::Levels papbEmptyState = gpio::Levels::HIGH;
 
   result = gpioComIF->readGpio(papbEmptyId, papbEmptyState);
 
   if (result != returnvalue::OK) {
-    sif::warning << "PapbVcInterface::isVcInterfaceBufferEmpty: Failed to read papb empty signal"
-                 << std::endl;
-    return;
+    sif::error << "PapbVcInterface::isVcInterfaceBufferEmpty: Failed to read papb empty signal"
+               << std::endl;
+    return true;
   }
 
   if (papbEmptyState == gpio::Levels::HIGH) {
-    sif::debug << "PapbVcInterface::isVcInterfaceBufferEmpty: Buffer is empty" << std::endl;
-  } else {
-    sif::debug << "PapbVcInterface::isVcInterfaceBufferEmpty: Buffer is not empty" << std::endl;
+    return true;
   }
-  return;
+  return false;
 }
 
-bool PapbVcInterface::isBusy() const { return pollInterfaceReadiness(0, true) == PAPB_BUSY; }
+bool PapbVcInterface::isBusy() const { return not pollReadyForPacket(); }
 
 void PapbVcInterface::cancelTransfer() { abortPacketTransfer(); }
 
+inline bool PapbVcInterface::pollReadyForOctet(uint32_t maxCycles) const {
+  uint32_t reg;
+  uint32_t idx = 0;
+  while (idx < maxCycles) {
+    reg = *vcBaseReg;
+    // Busy bit.
+    if (not((reg >> 5) & 0b1)) {
+      return true;
+    }
+    idx++;
+  }
+  return false;
+}
+
 ReturnValue_t PapbVcInterface::sendTestFrame() {
   /** Size of one complete transfer frame data field amounts to 1105 bytes */
   uint8_t testPacket[1105];

linux/ipcore/PapbVcInterface.h

@@ -30,8 +30,7 @@ class PapbVcInterface : public VirtualChannelIF {
    * @param uioFile       UIO file providing access to the PAPB bus
    * @param mapNum        Map number of UIO map associated with this virtual channel
    */
-  PapbVcInterface(LinuxLibgpioIF* gpioComIF, gpioId_t papbBusyId, gpioId_t papbEmptyId,
-                  std::string uioFile, int mapNum);
+  PapbVcInterface(LinuxLibgpioIF* gpioComIF, gpioId_t papbEmptyId, std::string uioFile, int mapNum);
   virtual ~PapbVcInterface();
 
   bool isBusy() const override;
@@ -81,11 +80,9 @@ class PapbVcInterface : public VirtualChannelIF {
 
   static constexpr long int FIRST_DELAY_PAPB_POLLING_NS = 10;
   static constexpr long int MAX_DELAY_PAPB_POLLING_NS = 40;
+  static constexpr uint32_t MAX_BUSY_POLLS = 1000;
 
   LinuxLibgpioIF* gpioComIF = nullptr;
-
-  /** Pulled to low when virtual channel not ready to receive data */
-  gpioId_t papbBusyId = gpio::NO_GPIO;
   /** High when external buffer memory of virtual channel is empty */
   gpioId_t papbEmptyId = gpio::NO_GPIO;
 
@@ -120,13 +117,15 @@ class PapbVcInterface : public VirtualChannelIF {
    *
    * @return  returnvalue::OK when ready to receive data else PAPB_BUSY.
    */
-  inline ReturnValue_t pollInterfaceReadiness(uint32_t maxPollRetries, bool checkReadyState) const;
+  inline bool pollReadyForPacket() const;
+
+  inline bool pollReadyForOctet(uint32_t maxCycles) const;
 
   /**
    * @brief   This function can be used for debugging to check whether there are packets in
    *          the packet buffer of the virtual channel or not.
    */
-  void isVcInterfaceBufferEmpty();
+  bool isVcInterfaceBufferEmpty();
 
   /**
    * @brief   This function sends a complete telemetry transfer frame data field (1105 bytes)

linux/ipcore/PtmeConfig.cpp

@@ -26,6 +26,11 @@ ReturnValue_t PtmeConfig::setRate(uint32_t bitRate) {
   return axiPtmeConfig->writeCaduRateReg(static_cast<uint8_t>(rateVal));
 }
 
+uint32_t PtmeConfig::getRate() {
+  uint8_t rateReg = axiPtmeConfig->readCaduRateReg();
+  return (BIT_CLK_FREQ / (rateReg + 1));
+}
+
 void PtmeConfig::invertTxClock(bool invert) {
   if (invert) {
     axiPtmeConfig->enableTxclockInversion();

linux/ipcore/PtmeConfig.h

@@ -32,6 +32,7 @@ class PtmeConfig : public SystemObject {
    *          of the CADU clock due to the convolutional code added by the s-Band transceiver.
    */
   ReturnValue_t setRate(uint32_t bitRate);
+  uint32_t getRate();
 
   /**
    * @brief Will change the time the tx data signal is updated with respect to the tx clock

mission/com/CcsdsIpCoreHandler.cpp

@@ -246,7 +246,13 @@ ReturnValue_t CcsdsIpCoreHandler::checkModeCommand(Mode_t mode, Submode_t submod
 void CcsdsIpCoreHandler::startTransition(Mode_t mode, Submode_t submode) {
   triggerEvent(CHANGING_MODE, mode, submode);
   if (mode == HasModesIF::MODE_ON) {
-    if (this->submode != submode) {
+    uint32_t currentRate = ptmeConfig.getRate();
+    // Check whether the rate actually changes.
+    if ((this->submode != submode) and
+        (((submode == static_cast<Submode_t>(com::CcsdsSubmode::DATARATE_LOW) and
+           (currentRate != RATE_100KBPS))) or
+         ((submode == static_cast<Submode_t>(com::CcsdsSubmode::DATARATE_HIGH) and
+           (currentRate != RATE_500KBPS))))) {
       initPtmeUpdateAfterXCycles();
       updateContext.enableTransmitAfterPtmeUpdate = true;
       updateContext.updateClockRate = true;

mission/com/LiveTmTask.cpp

@@ -19,13 +19,8 @@ LiveTmTask::LiveTmTask(object_id_t objectId, PusTmFunnel& pusFunnel, CfdpTmFunne
 ReturnValue_t LiveTmTask::performOperation(uint8_t opCode) {
   readCommandQueue();
   while (true) {
-    bool performWriteOp = true;
-    if (mode == MODE_OFF or ptmeLocked) {
-      performWriteOp = false;
-    }
-
     // The funnel tasks are scheduled here directly as well.
-    ReturnValue_t result = channel.handleNextTm(performWriteOp);
+    ReturnValue_t result = channel.handleNextTm(!ptmeLocked);
     if (result == DirectTmSinkIF::IS_BUSY) {
       sif::error << "Lost live TM, PAPB busy" << std::endl;
     }

mission/com/PersistentLogTmStoreTask.cpp

@@ -42,13 +42,7 @@ ReturnValue_t PersistentLogTmStoreTask::performOperation(uint8_t opCode) {
     if (not someonesBusy) {
       TaskFactory::delayTask(100);
     } else if (vcBusyDuringDump) {
-      // TODO: Might not be necessary
-      sif::debug << "VC busy, delaying" << std::endl;
       TaskFactory::delayTask(10);
-    } else {
-      // TODO: Would be best to remove this, but not delaying here can lead to evil issues.
-      // Polling the PAPB of the PTME core too often leads to scheuduling issues.
-      TaskFactory::delayTask(2);
     }
   }
 }

mission/com/PersistentSingleTmStoreTask.cpp

@@ -24,13 +24,7 @@ ReturnValue_t PersistentSingleTmStoreTask::performOperation(uint8_t opCode) {
     if (not busy) {
       TaskFactory::delayTask(100);
     } else if (dumpContext.vcBusyDuringDump) {
-      sif::debug << "VC busy, delaying" << std::endl;
-      // TODO: Might not be necessary
       TaskFactory::delayTask(10);
-    } else {
-      // TODO: Would be best to remove this, but not delaying here can lead to evil issues.
-      // Polling the PAPB of the PTME core too often leads to scheuduling issues.
-      TaskFactory::delayTask(2);
     }
   }
 }

mission/com/SyrlinksHandler.cpp

@@ -773,11 +773,13 @@ void SyrlinksHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) {
   auto txStandbyHandler = [&]() {
     txDataset.setReportingEnabled(false);
     poolManager.changeCollectionInterval(temperatureSet.getSid(), 60.0);
+    poolManager.changeCollectionInterval(rxDataset.getSid(), 60.0);
     transState = TransitionState::SET_TX_STANDBY;
     internalState = InternalState::TX_TRANSITION;
   };
   auto txOnHandler = [&](TransitionState tgtTransitionState) {
     txDataset.setReportingEnabled(true);
+    poolManager.changeCollectionInterval(rxDataset.getSid(), 5.0);
     poolManager.changeCollectionInterval(txDataset.getSid(), 10.0);
     poolManager.changeCollectionInterval(temperatureSet.getSid(), 5.0);
     transState = tgtTransitionState;

mission/com/TmStoreTaskBase.cpp

@@ -45,13 +45,19 @@ bool TmStoreTaskBase::handleOneStore(PersistentTmStoreWithTmQueue& store,
   } else {
     Command_t execCmd;
     // Handle TC requests, for example deletion or retrieval requests.
+    // TODO: Not really clean here.. would be better if the executed command is returns as an
+    // enumeration.
     result = store.handleCommandQueue(ipcStore, execCmd);
-    if (result == returnvalue::OK) {
-      if (execCmd == TmStoreMessage::DOWNLINK_STORE_CONTENT_TIME) {
+    if (execCmd == TmStoreMessage::DOWNLINK_STORE_CONTENT_TIME) {
+      if (result == PersistentTmStore::DUMP_DONE) {
+        dumpDoneHandler(store, dumpContext);
+      } else if (result == returnvalue::OK) {
         cancelDumpCd.resetTimer();
         tmSinkBusyCd.resetTimer();
         dumpContext.reset();
       }
+    }
+    if (execCmd != CommandMessageIF::CMD_NONE) {
       tcRequestReceived = true;
     }
   }
@@ -119,21 +125,13 @@ ReturnValue_t TmStoreTaskBase::performDump(PersistentTmStoreWithTmQueue& store,
                                            DumpContext& dumpContext, bool& dumpPerformed) {
   size_t dumpedLen = 0;
 
-  auto dumpDoneHandler = [&]() {
-    uint32_t startTime;
-    uint32_t endTime;
-    store.getStartAndEndTimeCurrentOrLastDump(startTime, endTime);
-    triggerEvent(dumpContext.eventIfDone, dumpContext.numberOfDumpedPackets,
-                 dumpContext.dumpedBytes);
-    dumpContext.reset();
-  };
   // Dump the next packet into the PTME.
   dumpContext.ptmeBusyCounter = 0;
   tmSinkBusyCd.resetTimer();
   ReturnValue_t result = store.getNextDumpPacket(tmReader, fileHasSwapped);
   if (fileHasSwapped and result == PersistentTmStore::DUMP_DONE) {
     // This can happen if a file is corrupted and the next file swap completes the dump.
-    dumpDoneHandler();
+    dumpDoneHandler(store, dumpContext);
     return returnvalue::OK;
   } else if (result != returnvalue::OK) {
     sif::error << "PersistentTmStore: Getting next dump packet failed" << std::endl;
@@ -157,7 +155,7 @@ ReturnValue_t TmStoreTaskBase::performDump(PersistentTmStoreWithTmQueue& store,
     }
   }
   if (result == PersistentTmStore::DUMP_DONE) {
-    dumpDoneHandler();
+    dumpDoneHandler(store, dumpContext);
   }
   return returnvalue::OK;
 }
@@ -198,6 +196,14 @@ ReturnValue_t TmStoreTaskBase::connectModeTreeParent(HasModeTreeChildrenIF& pare
   return modetree::connectModeTreeParent(parent, *this, nullptr, modeHelper);
 }
 
+void TmStoreTaskBase::dumpDoneHandler(PersistentTmStore& store, DumpContext& dumpContext) {
+  uint32_t startTime;
+  uint32_t endTime;
+  store.getStartAndEndTimeCurrentOrLastDump(startTime, endTime);
+  triggerEvent(dumpContext.eventIfDone, dumpContext.numberOfDumpedPackets, dumpContext.dumpedBytes);
+  dumpContext.reset();
+}
+
 ModeTreeChildIF& TmStoreTaskBase::getModeTreeChildIF() { return *this; }
 
 void TmStoreTaskBase::readCommandQueue(void) {

mission/com/TmStoreTaskBase.h

@@ -96,6 +96,8 @@ class TmStoreTaskBase : public SystemObject,
 
   ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode,
                                  uint32_t* msToReachTheMode) override;
+  void dumpDoneHandler(PersistentTmStore& store, DumpContext& dumpContext);
+
   void announceMode(bool recursive) override;
   object_id_t getObjectId() const override;
   const HasHealthIF* getOptHealthIF() const override;

mission/genericFactory.cpp

@@ -91,19 +91,21 @@ EiveFaultHandler EIVE_FAULT_HANDLER;
 }  // namespace cfdp
 
 std::atomic_bool tcs::TCS_BOARD_SHORTLY_UNAVAILABLE = false;
+std::atomic_bool core::SAVE_PUS_SEQUENCE_COUNT = false;
+std::atomic_bool core::SAVE_CFDP_SEQUENCE_COUNT = false;
 
 void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFunnel** pusFunnel,
                                           CfdpTmFunnel** cfdpFunnel, SdCardMountedIF& sdcMan,
                                           StorageManagerIF** ipcStore, StorageManagerIF** tmStore,
                                           PersistentTmStores& stores,
-                                          uint32_t eventManagerQueueDepth) {
+                                          uint32_t eventManagerQueueDepth, bool enableHkSets) {
   // Framework objects
   new EventManager(objects::EVENT_MANAGER, eventManagerQueueDepth);
   auto healthTable = new HealthTable(objects::HEALTH_TABLE);
   if (healthTable_ != nullptr) {
     *healthTable_ = healthTable;
   }
-  new InternalErrorReporter(objects::INTERNAL_ERROR_REPORTER);
+  new InternalErrorReporter(objects::INTERNAL_ERROR_REPORTER, 5, enableHkSets, 120);
   new VerificationReporter();
   auto* timeStamper = new CdsShortTimeStamper(objects::TIME_STAMPER);
   StorageManagerIF* tcStore;
@@ -155,9 +157,11 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun
   new PusDistributor(config::EIVE_PUS_APID, objects::PUS_PACKET_DISTRIBUTOR, ccsdsDistrib);
 
   PusTmFunnel::FunnelCfg pusFunnelCfg(objects::PUS_TM_FUNNEL, "PusTmFunnel", **tmStore, **ipcStore,
-                                      config::MAX_PUS_FUNNEL_QUEUE_DEPTH);
+                                      config::MAX_PUS_FUNNEL_QUEUE_DEPTH, sdcMan,
+                                      config::PUS_SEQUENCE_COUNT_FILE,
+                                      core::SAVE_PUS_SEQUENCE_COUNT);
   // The PUS funnel routes all live TM to the live destinations and to the TM stores.
-  *pusFunnel = new PusTmFunnel(pusFunnelCfg, *ramToFileStore, *timeStamper, sdcMan);
+  *pusFunnel = new PusTmFunnel(pusFunnelCfg, *ramToFileStore, *timeStamper);
 
   // MISC store and PUS funnel to MISC store routing
   {
@@ -216,7 +220,9 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun
                                       stores.cfdpStore->getReportReceptionQueue(0));
   }
   PusTmFunnel::FunnelCfg cfdpFunnelCfg(objects::CFDP_TM_FUNNEL, "CfdpTmFunnel", **tmStore,
-                                       **ipcStore, config::MAX_CFDP_FUNNEL_QUEUE_DEPTH);
+                                       **ipcStore, config::MAX_CFDP_FUNNEL_QUEUE_DEPTH, sdcMan,
+                                       config::CFDP_SEQUENCE_COUNT_FILE,
+                                       core::SAVE_CFDP_SEQUENCE_COUNT);
   *cfdpFunnel = new CfdpTmFunnel(cfdpFunnelCfg, stores.cfdpStore->getReportReceptionQueue(0),
                                  *ramToFileStore, config::EIVE_CFDP_APID);
 

mission/genericFactory.h

@@ -45,7 +45,8 @@ namespace ObjectFactory {
 void produceGenericObjects(HealthTableIF** healthTable, PusTmFunnel** pusFunnel,
                            CfdpTmFunnel** cfdpFunnel, SdCardMountedIF& sdcMan,
                            StorageManagerIF** ipcStore, StorageManagerIF** tmStore,
-                           PersistentTmStores& stores, uint32_t eventManagerQueueDepth);
+                           PersistentTmStores& stores, uint32_t eventManagerQueueDepth,
+                           bool enableHkSets);
 void createGenericHeaterComponents(GpioIF& gpioIF, PowerSwitchIF& pwrSwitcher,
                                    HeaterHandler*& heaterHandler);
 

mission/sysDefs.h

@@ -34,6 +34,9 @@ enum Copy : int { COPY_0, COPY_1, NO_COPY, SELF_COPY, ALL_COPY };
 
 namespace core {
 
+extern std::atomic_bool SAVE_PUS_SEQUENCE_COUNT;
+extern std::atomic_bool SAVE_CFDP_SEQUENCE_COUNT;
+
 // TODO: Support for status? Or maybe some command to quickly get information whether a unit
 // is running.
 enum SystemctlCmd : uint8_t { START = 0, STOP = 1, RESTART = 2, NUM_CMDS = 3 };

mission/tcs/HeaterHandler.cpp

@@ -51,9 +51,13 @@ ReturnValue_t HeaterHandler::performOperation(uint8_t operationCode) {
       if (mainLineSwitcher->getSwitchState(mainLineSwitch) == SWITCH_OFF) {
         waitForSwitchOff = false;
         mode = MODE_OFF;
+        busyWithSwitchCommanding = false;
         modeHelper.modeChanged(mode, submode);
       }
     }
+    if (busyWithSwitchCommanding and heaterCmdBusyCd.hasTimedOut()) {
+      busyWithSwitchCommanding = false;
+    }
   } catch (const std::out_of_range& e) {
     sif::warning << "HeaterHandler::performOperation: "
                     "Out of range error | "
@@ -101,23 +105,23 @@ ReturnValue_t HeaterHandler::initializeHeaterMap() {
 void HeaterHandler::readCommandQueue() {
   ReturnValue_t result = returnvalue::OK;
   CommandMessage command;
-  do {
+  if (not busyWithSwitchCommanding) {
     result = commandQueue->receiveMessage(&command);
     if (result == MessageQueueIF::EMPTY) {
-      break;
+      return;
     } else if (result != returnvalue::OK) {
       sif::warning << "HeaterHandler::readCommandQueue: Message reception error" << std::endl;
-      break;
-    }
-    result = actionHelper.handleActionMessage(&command);
-    if (result == returnvalue::OK) {
-      continue;
+      return;
     }
     result = modeHelper.handleModeCommand(&command);
     if (result == returnvalue::OK) {
-      continue;
+      return;
     }
-  } while (result == returnvalue::OK);
+    result = actionHelper.handleActionMessage(&command);
+    if (result == returnvalue::OK) {
+      return;
+    }
+  }
 }
 
 ReturnValue_t HeaterHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
@@ -167,6 +171,8 @@ ReturnValue_t HeaterHandler::executeAction(ActionId_t actionId, MessageQueueId_t
   heater.action = action;
   heater.cmdActive = true;
   heater.replyQueue = commandedBy;
+  busyWithSwitchCommanding = true;
+  heaterCmdBusyCd.resetTimer();
   return returnvalue::OK;
 }
 
@@ -249,6 +255,7 @@ void HeaterHandler::handleSwitchOnCommand(heater::Switch heaterIdx) {
     sif::error << "HeaterHandler::handleSwitchOnCommand: Main switch setting on timeout"
                << std::endl;
     heater.cmdActive = false;
+    busyWithSwitchCommanding = false;
     heater.waitMainSwitchOn = false;
     if (heater.replyQueue != commandQueue->getId()) {
       actionHelper.finish(false, heater.replyQueue, heater.action, MAIN_SWITCH_SET_TIMEOUT);
@@ -259,27 +266,25 @@ void HeaterHandler::handleSwitchOnCommand(heater::Switch heaterIdx) {
   // Check state of main line switch
   ReturnValue_t mainSwitchState = mainLineSwitcher->getSwitchState(mainLineSwitch);
   if (mainSwitchState == PowerSwitchIF::SWITCH_ON) {
-    if (getSwitchState(heaterIdx) == SwitchState::OFF) {
-      gpioId_t gpioId = heater.gpioId;
-      result = gpioInterface->pullHigh(gpioId);
-      if (result != returnvalue::OK) {
-        sif::error << "HeaterHandler::handleSwitchOnCommand: Failed to pull gpio with id " << gpioId
-                   << " high" << std::endl;
-        triggerEvent(GPIO_PULL_HIGH_FAILED, result);
-      } else {
-        triggerEvent(HEATER_WENT_ON, heaterIdx, 0);
-        EventManagerIF::triggerEvent(helper.heaters[heaterIdx].first->getObjectId(), MODE_INFO,
-                                     MODE_ON, 0);
-        {
-          MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
-          heater.switchState = ON;
-        }
-      }
-    } else {
-      triggerEvent(SWITCH_ALREADY_ON, heaterIdx);
+    gpioId_t gpioId = heater.gpioId;
+    result = gpioInterface->pullHigh(gpioId);
+    if (result != returnvalue::OK) {
+      sif::error << "HeaterHandler::handleSwitchOnCommand: Failed to pull GPIO with ID " << gpioId
+                 << " high" << std::endl;
+      triggerEvent(GPIO_PULL_HIGH_FAILED, result);
+    }
+    if (result == returnvalue::OK) {
+      triggerEvent(HEATER_WENT_ON, heaterIdx, 0);
+      {
+        MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
+        heater.switchState = ON;
+      }
+      EventManagerIF::triggerEvent(helper.heaters[heaterIdx].first->getObjectId(), MODE_INFO,
+                                   MODE_ON, 0);
+      busyWithSwitchCommanding = false;
+      mode = HasModesIF::MODE_ON;
+      modeHelper.modeChanged(mode, submode);
     }
-    mode = HasModesIF::MODE_ON;
-    modeHelper.modeChanged(mode, submode);
     // There is no need to send action finish replies if the sender was the
     // HeaterHandler itself
     if (heater.replyQueue != commandQueue->getId()) {
@@ -312,30 +317,33 @@ void HeaterHandler::handleSwitchOnCommand(heater::Switch heaterIdx) {
 void HeaterHandler::handleSwitchOffCommand(heater::Switch heaterIdx) {
   ReturnValue_t result = returnvalue::OK;
   auto& heater = heaterVec.at(heaterIdx);
-  // Check whether switch is already off
-  if (getSwitchState(heaterIdx)) {
-    gpioId_t gpioId = heater.gpioId;
-    result = gpioInterface->pullLow(gpioId);
-    if (result != returnvalue::OK) {
-      sif::error << "HeaterHandler::handleSwitchOffCommand: Failed to pull gpio with id" << gpioId
-                 << " low" << std::endl;
-      triggerEvent(GPIO_PULL_LOW_FAILED, result);
-    } else {
+  gpioId_t gpioId = heater.gpioId;
+  result = gpioInterface->pullLow(gpioId);
+  if (result != returnvalue::OK) {
+    sif::error << "HeaterHandler::handleSwitchOffCommand: Failed to pull gpio with id" << gpioId
+               << " low" << std::endl;
+    triggerEvent(GPIO_PULL_LOW_FAILED, result);
+  }
+  if (result == returnvalue::OK) {
+    // Check whether switch is already off
+    if (getSwitchState(heaterIdx) == SwitchState::ON) {
       {
         MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
         heater.switchState = OFF;
       }
       triggerEvent(HEATER_WENT_OFF, heaterIdx, 0);
-      EventManagerIF::triggerEvent(helper.heaters[heaterIdx].first->getObjectId(), MODE_INFO,
-                                   MODE_OFF, 0);
-      // When all switches are off, also main line switch will be turned off
-      if (allSwitchesOff()) {
-        mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF);
-        waitForSwitchOff = true;
-      }
+    } else {
+      triggerEvent(SWITCH_ALREADY_OFF, heaterIdx);
+    }
+    EventManagerIF::triggerEvent(helper.heaters[heaterIdx].first->getObjectId(), MODE_INFO,
+                                 MODE_OFF, 0);
+    // When all switches are off, also main line switch will be turned off
+    if (allSwitchesOff()) {
+      mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF);
+      waitForSwitchOff = true;
+    } else {
+      busyWithSwitchCommanding = false;
     }
-  } else {
-    triggerEvent(SWITCH_ALREADY_OFF, heaterIdx);
   }
   if (heater.replyQueue != NO_COMMANDER) {
     // Report back switch command reply if necessary

mission/tcs/HeaterHandler.h

@@ -148,6 +148,7 @@ class HeaterHandler : public ExecutableObjectIF,
   /** Size of command queue */
   size_t cmdQueueSize = 20;
   bool waitForSwitchOff = true;
+  bool busyWithSwitchCommanding = false;
 
   GpioIF* gpioInterface = nullptr;
 
@@ -163,6 +164,7 @@ class HeaterHandler : public ExecutableObjectIF,
   power::Switch_t mainLineSwitch;
 
   ActionHelper actionHelper;
+  Countdown heaterCmdBusyCd = Countdown(2000);
 
   StorageManagerIF* ipcStore = nullptr;
 

mission/tmtc/CfdpTmFunnel.cpp

@@ -15,8 +15,16 @@ const char* CfdpTmFunnel::getName() const { return "CFDP TM Funnel"; }
 
 ReturnValue_t CfdpTmFunnel::performOperation(uint8_t) {
   TmTcMessage currentMessage;
+  ReturnValue_t status;
   unsigned int count = 0;
-  ReturnValue_t status = tmQueue->receiveMessage(&currentMessage);
+  if (saveSequenceCount) {
+    status = saveSequenceCountToFile();
+    if (status != returnvalue::OK) {
+      sif::error << "CfdpTmFunnel: Storing sequence count to file has failed" << std::endl;
+    }
+    saveSequenceCount = false;
+  }
+  status = tmQueue->receiveMessage(&currentMessage);
   while (status == returnvalue::OK) {
     status = handlePacket(currentMessage);
     if (status != returnvalue::OK) {

mission/tmtc/CfdpTmFunnel.h

@@ -3,6 +3,7 @@
 
 #include <mission/tmtc/TmFunnelBase.h>
 
+#include <atomic>
 #include <vector>
 
 #include "fsfw/objectmanager/SystemObject.h"
@@ -23,7 +24,6 @@ class CfdpTmFunnel : public TmFunnelBase {
 
   MessageQueueId_t fileStoreDest;
   StorageManagerIF& ramToFileStore;
-  uint16_t sourceSequenceCount = 0;
   uint16_t cfdpInCcsdsApid;
 };
 #endif  // FSFW_EXAMPLE_COMMON_CFDPTMFUNNEL_H

mission/tmtc/PersistentTmStore.cpp

@@ -17,6 +17,8 @@
 
 using namespace returnvalue;
 
+static constexpr bool DEBUG_DUMPS = false;
+
 PersistentTmStore::PersistentTmStore(PersistentTmStoreArgs args)
     : SystemObject(args.objectId),
       tmStore(args.tmStore),
@@ -32,6 +34,91 @@ ReturnValue_t PersistentTmStore::cancelDump() {
   return returnvalue::OK;
 }
 
+ReturnValue_t PersistentTmStore::buildDumpSet(uint32_t fromUnixSeconds, uint32_t upToUnixSeconds) {
+  using namespace std::filesystem;
+  std::error_code e;
+  dumpParams.orderedDumpFilestamps.clear();
+  for (auto const& fileOrDir : directory_iterator(basePath)) {
+    if (not fileOrDir.is_regular_file(e)) {
+      continue;
+    }
+    dumpParams.fileSize = std::filesystem::file_size(fileOrDir.path(), e);
+    if (e) {
+      sif::error << "PersistentTmStore: Could not retrieve file size: " << e.message() << std::endl;
+      continue;
+    }
+
+    // File empty or can't even read CCSDS header.
+    if (dumpParams.fileSize <= 6) {
+      continue;
+    }
+    if (dumpParams.fileSize > fileBuf.size()) {
+      sif::error << "PersistentTmStore: File too large, is deleted" << std::endl;
+      triggerEvent(persTmStore::FILE_TOO_LARGE, dumpParams.fileSize, fileBuf.size());
+      std::filesystem::remove(fileOrDir.path(), e);
+      continue;
+    }
+    const path& file = fileOrDir.path();
+    struct tm fileTime {};
+    if (pathToTime(file, fileTime) != returnvalue::OK) {
+      sif::error << "Time extraction for file " << file << "failed" << std::endl;
+      continue;
+    }
+    auto fileEpoch = static_cast<uint32_t>(timegm(&fileTime));
+    if ((fileEpoch > dumpParams.fromUnixTime) and
+        (fileEpoch + rolloverDiffSeconds <= dumpParams.untilUnixTime)) {
+      std::ifstream ifile(file, std::ios::binary);
+      if (ifile.bad()) {
+        sif::error << "PersistentTmStore: File is bad" << std::endl;
+        // TODO: Consider deleting file here?
+        continue;
+      }
+
+      if (DEBUG_DUMPS) {
+        sif::debug << "Inserting file " << fileOrDir.path() << std::endl;
+      }
+      DumpIndex dumpIndex;
+      dumpIndex.epoch = fileEpoch;
+      // Multiple files for the same time are supported via a special suffix. We simply count the
+      // number of copies and later try to dump the same number of files with the additional
+      // suffixes
+      auto iter = dumpParams.orderedDumpFilestamps.find(dumpIndex);
+      if (iter != dumpParams.orderedDumpFilestamps.end()) {
+        dumpIndex.epoch = iter->epoch;
+        dumpIndex.additionalFiles = iter->additionalFiles + 1;
+        dumpParams.orderedDumpFilestamps.erase(dumpIndex);
+      } else {
+        dumpIndex.additionalFiles = 0;
+      }
+      dumpParams.orderedDumpFilestamps.emplace(dumpIndex);
+    }
+  }
+  return returnvalue::OK;
+}
+
+std::optional<uint8_t> PersistentTmStore::extractSuffix(const std::string& pathStr) {
+  std::string numberStr;
+  // Find the position of the dot at the end of the file path
+  size_t dotPos = pathStr.find_last_of('.');
+  if ((dotPos < pathStr.length()) and not std::isdigit(pathStr[dotPos + 1])) {
+    return std::nullopt;
+  }
+  // Extract the substring after the dot
+  numberStr = pathStr.substr(dotPos + 1);
+  std::optional<uint8_t> number;
+  try {
+    number = std::stoi(numberStr);
+    if (number.value() > std::numeric_limits<uint8_t>::max()) {
+      return std::nullopt;
+    }
+
+  } catch (std::invalid_argument& exception) {
+    sif::error << "PersistentTmStore::extractSuffix: Exception " << exception.what()
+               << ", invald input string: " << numberStr << std::endl;
+  }
+  return number;
+}
+
 ReturnValue_t PersistentTmStore::assignAndOrCreateMostRecentFile() {
   if (not activeFile.has_value()) {
     return createMostRecentFile(std::nullopt);
@@ -41,6 +128,7 @@ ReturnValue_t PersistentTmStore::assignAndOrCreateMostRecentFile() {
 
 ReturnValue_t PersistentTmStore::handleCommandQueue(StorageManagerIF& ipcStore,
                                                     Command_t& execCmd) {
+  execCmd = CommandMessageIF::CMD_NONE;
   CommandMessage cmdMessage;
   ReturnValue_t result = tcQueue->receiveMessage(&cmdMessage);
   if (result != returnvalue::OK) {
@@ -75,9 +163,9 @@ ReturnValue_t PersistentTmStore::handleCommandQueue(StorageManagerIF& ipcStore,
       result = startDumpFromUpTo(dumpFromUnixSeconds, dumpUntilUnixSeconds);
       if (result == BUSY_DUMPING) {
         triggerEvent(persTmStore::BUSY_DUMPING_EVENT);
-      } else {
-        execCmd = cmd;
+        return result;
       }
+      execCmd = cmd;
     }
   }
   return result;
@@ -159,6 +247,12 @@ bool PersistentTmStore::updateBaseDir() {
   if (not exists(basePath, e)) {
     create_directories(basePath);
   }
+  // Each file will have the base name as a prefix again
+  path preparedFullFilePath = basePath / baseName;
+  basePathSize = preparedFullFilePath.string().length();
+  std::memcpy(filePathBuf.data(), preparedFullFilePath.c_str(), basePathSize);
+  filePathBuf[basePathSize] = '_';
+  basePathSize += 1;
   baseDirUninitialized = false;
   return true;
 }
@@ -189,12 +283,20 @@ ReturnValue_t PersistentTmStore::startDumpFromUpTo(uint32_t fromUnixSeconds,
   if (state == State::DUMPING) {
     return returnvalue::FAILED;
   }
-  dumpParams.dirIter = directory_iterator(basePath);
-  if (dumpParams.dirIter == directory_iterator()) {
+  auto dirIter = directory_iterator(basePath);
+  // Directory empty case.
+  if (dirIter == directory_iterator()) {
     return returnvalue::FAILED;
   }
   dumpParams.fromUnixTime = fromUnixSeconds;
   dumpParams.untilUnixTime = upToUnixSeconds;
+  buildDumpSet(fromUnixSeconds, upToUnixSeconds);
+  // No files in time window found.
+  if (dumpParams.orderedDumpFilestamps.empty()) {
+    return DUMP_DONE;
+  }
+  dumpParams.dumpIter = dumpParams.orderedDumpFilestamps.begin();
+  dumpParams.currentSameFileIdx = std::nullopt;
   state = State::DUMPING;
   return loadNextDumpFile();
 }
@@ -203,49 +305,54 @@ ReturnValue_t PersistentTmStore::loadNextDumpFile() {
   using namespace std::filesystem;
   dumpParams.currentSize = 0;
   std::error_code e;
-  for (; dumpParams.dirIter != directory_iterator(); dumpParams.dirIter++) {
-    dumpParams.dirEntry = *dumpParams.dirIter;
-    if (dumpParams.dirEntry.is_directory(e)) {
-      continue;
-    }
-    dumpParams.fileSize = std::filesystem::file_size(dumpParams.dirEntry.path(), e);
-    if (e) {
-      sif::error << "PersistentTmStore: Could not retrieve file size: " << e.message() << std::endl;
-      continue;
-    }
-    // sif::debug << "Path: " << dumpParams.dirEntry.path() << std::endl;
-
-    // File empty or can't even read CCSDS header.
-    if (dumpParams.fileSize <= 6) {
-      continue;
-    }
-    if (dumpParams.fileSize > fileBuf.size()) {
-      sif::error << "PersistentTmStore: File too large, is deleted" << std::endl;
-      triggerEvent(persTmStore::FILE_TOO_LARGE, dumpParams.fileSize, fileBuf.size());
-      std::filesystem::remove(dumpParams.dirEntry.path(), e);
-      continue;
-    }
-    const path& file = dumpParams.dirEntry.path();
-    struct tm fileTime {};
-    if (pathToTime(file, fileTime) != returnvalue::OK) {
-      sif::error << "Time extraction for file " << file << "failed" << std::endl;
-      continue;
-    }
-    auto fileEpoch = static_cast<uint32_t>(timegm(&fileTime));
-    if ((fileEpoch > dumpParams.fromUnixTime) and
-        (fileEpoch + rolloverDiffSeconds <= dumpParams.untilUnixTime)) {
-      dumpParams.currentFileUnixStamp = fileEpoch;
-      std::ifstream ifile(file, std::ios::binary);
-      if (ifile.bad()) {
-        sif::error << "PersistentTmStore: File is bad" << std::endl;
-        continue;
+  // Handle iteration, which does not necessarily have to increment the set iterator
+  // if there are multiple files for a given timestamp.
+  auto handleIteration = [&](DumpIndex& dumpIndex) {
+    if (dumpIndex.additionalFiles > 0) {
+      if (not dumpParams.currentSameFileIdx.has_value()) {
+        // Initialize the file index and stay on same file
+        dumpParams.currentSameFileIdx = 0;
+        return;
+      } else if (dumpParams.currentSameFileIdx.value() < dumpIndex.additionalFiles - 1) {
+        dumpParams.currentSameFileIdx = dumpParams.currentSameFileIdx.value() + 1;
+        return;
       }
-      ifile.read(reinterpret_cast<char*>(fileBuf.data()),
-                 static_cast<std::streamsize>(dumpParams.fileSize));
-      // Increment iterator for next cycle.
-      dumpParams.dirIter++;
-      return returnvalue::OK;
     }
+    // File will change, reset this field for correct state-keeping.
+    dumpParams.currentSameFileIdx = std::nullopt;
+    // Increment iterator for next cycle.
+    dumpParams.dumpIter++;
+  };
+  while (dumpParams.dumpIter != dumpParams.orderedDumpFilestamps.end()) {
+    DumpIndex dumpIndex = *dumpParams.dumpIter;
+    timeval tv{};
+    tv.tv_sec = dumpIndex.epoch;
+    size_t fullPathLength = 0;
+    createFileName(tv, dumpParams.currentSameFileIdx, fullPathLength);
+    dumpParams.currentFile =
+        path(std::string(reinterpret_cast<const char*>(filePathBuf.data()), fullPathLength));
+    if (DEBUG_DUMPS) {
+      sif::debug << baseName << " dump: Loading " << dumpParams.currentFile << std::endl;
+    }
+    dumpParams.fileSize = std::filesystem::file_size(dumpParams.currentFile, e);
+    if (e) {
+      // TODO: Event?
+      sif::error << "PersistentTmStore: Could not load next dump file: " << e.message()
+                 << std::endl;
+      handleIteration(dumpIndex);
+      continue;
+    }
+    std::ifstream ifile(dumpParams.currentFile, std::ios::binary);
+    if (ifile.bad()) {
+      sif::error << "PersistentTmStore: File is bad. Loading next file" << std::endl;
+      handleIteration(dumpIndex);
+      continue;
+    }
+    ifile.read(reinterpret_cast<char*>(fileBuf.data()),
+               static_cast<std::streamsize>(dumpParams.fileSize));
+    // Next file is loaded, exit the loop.
+    handleIteration(dumpIndex);
+    return returnvalue::OK;
   }
   // Directory iterator was consumed and we are done.
   state = State::IDLE;
@@ -253,7 +360,10 @@ ReturnValue_t PersistentTmStore::loadNextDumpFile() {
 }
 
 ReturnValue_t PersistentTmStore::getNextDumpPacket(PusTmReader& reader, bool& fileHasSwapped) {
-  if (state == State::IDLE or dumpParams.pendingPacketDump) {
+  if (state == State::IDLE) {
+    return DUMP_DONE;
+  }
+  if (dumpParams.pendingPacketDump) {
     return returnvalue::FAILED;
   }
   fileHasSwapped = false;
@@ -267,8 +377,8 @@ ReturnValue_t PersistentTmStore::getNextDumpPacket(PusTmReader& reader, bool& fi
     // Delete the file and load next. Could use better algorithm to partially
     // restore the file dump, but for now do not trust the file.
     std::error_code e;
-    std::filesystem::remove(dumpParams.dirEntry.path().c_str(), e);
-    if (dumpParams.dirEntry.path() == activeFile) {
+    std::filesystem::remove(dumpParams.currentFile.c_str(), e);
+    if (dumpParams.currentFile == activeFile) {
       activeFile == std::nullopt;
       assignAndOrCreateMostRecentFile();
     }
@@ -302,37 +412,9 @@ ReturnValue_t PersistentTmStore::pathToTime(const std::filesystem::path& path, s
 
 ReturnValue_t PersistentTmStore::createMostRecentFile(std::optional<uint8_t> suffix) {
   using namespace std::filesystem;
-  unsigned currentIdx = 0;
-  path pathStart = basePath / baseName;
-  memcpy(fileBuf.data() + currentIdx, pathStart.c_str(), pathStart.string().length());
-  currentIdx += pathStart.string().length();
-  fileBuf[currentIdx] = '_';
-  currentIdx += 1;
-  time_t epoch = currentTv.tv_sec;
-  struct tm* time = gmtime(&epoch);
-  size_t writtenBytes = strftime(reinterpret_cast<char*>(fileBuf.data() + currentIdx),
-                                 fileBuf.size(), config::FILE_DATE_FORMAT, time);
-  if (writtenBytes == 0) {
-    sif::error << "PersistentTmStore::createMostRecentFile: Could not create file timestamp"
-               << std::endl;
-    return returnvalue::FAILED;
-  }
-  currentIdx += writtenBytes;
-  char* res = strcpy(reinterpret_cast<char*>(fileBuf.data() + currentIdx), ".bin");
-  if (res == nullptr) {
-    return returnvalue::FAILED;
-  }
-  currentIdx += 4;
-  if (suffix.has_value()) {
-    std::string fullSuffix = "." + std::to_string(suffix.value());
-    res = strcpy(reinterpret_cast<char*>(fileBuf.data() + currentIdx), fullSuffix.c_str());
-    if (res == nullptr) {
-      return returnvalue::FAILED;
-    }
-    currentIdx += fullSuffix.size();
-  }
-
-  path newPath(std::string(reinterpret_cast<const char*>(fileBuf.data()), currentIdx));
+  size_t currentIdx;
+  createFileName(currentTv, suffix, currentIdx);
+  path newPath(std::string(reinterpret_cast<const char*>(filePathBuf.data()), currentIdx));
   std::ofstream of(newPath, std::ios::binary);
   activeFile = newPath;
   activeFileTv = currentTv;
@@ -354,3 +436,33 @@ void PersistentTmStore::getStartAndEndTimeCurrentOrLastDump(uint32_t& startTime,
   startTime = dumpParams.fromUnixTime;
   endTime = dumpParams.untilUnixTime;
 }
+
+ReturnValue_t PersistentTmStore::createFileName(timeval& tv, std::optional<uint8_t> suffix,
+                                                size_t& fullPathLength) {
+  unsigned currentIdx = basePathSize;
+  time_t epoch = tv.tv_sec;
+  struct tm* time = gmtime(&epoch);
+  size_t writtenBytes = strftime(reinterpret_cast<char*>(filePathBuf.data() + currentIdx),
+                                 filePathBuf.size(), config::FILE_DATE_FORMAT, time);
+  if (writtenBytes == 0) {
+    sif::error << "PersistentTmStore::createMostRecentFile: Could not create file timestamp"
+               << std::endl;
+    return returnvalue::FAILED;
+  }
+  currentIdx += writtenBytes;
+  char* res = strcpy(reinterpret_cast<char*>(filePathBuf.data() + currentIdx), ".bin");
+  if (res == nullptr) {
+    return returnvalue::FAILED;
+  }
+  currentIdx += 4;
+  if (suffix.has_value()) {
+    std::string fullSuffix = "." + std::to_string(suffix.value());
+    res = strcpy(reinterpret_cast<char*>(filePathBuf.data() + currentIdx), fullSuffix.c_str());
+    if (res == nullptr) {
+      return returnvalue::FAILED;
+    }
+    currentIdx += fullSuffix.size();
+  }
+  fullPathLength = currentIdx;
+  return returnvalue::OK;
+}

mission/tmtc/PersistentTmStore.h

@@ -10,6 +10,7 @@
 #include <mission/memory/SdCardMountedIF.h>
 
 #include <filesystem>
+#include <set>
 
 #include "eive/eventSubsystemIds.h"
 #include "eive/resultClassIds.h"
@@ -37,6 +38,14 @@ struct PersistentTmStoreArgs {
   SdCardMountedIF& sdcMan;
 };
 
+struct DumpIndex {
+  uint32_t epoch = 0;
+  // Number of additional files with a suffix like .0, .1 etc.
+  uint8_t additionalFiles = 0;
+  // Define a custom comparison function based on the epoch variable
+  bool operator<(const DumpIndex& other) const { return epoch < other.epoch; }
+};
+
 class PersistentTmStore : public TmStoreFrontendSimpleIF, public SystemObject {
  public:
   enum class State { IDLE, DUMPING };
@@ -96,7 +105,10 @@ class PersistentTmStore : public TmStoreFrontendSimpleIF, public SystemObject {
   std::string baseName;
   uint8_t currentSameSecNumber = 0;
   std::filesystem::path basePath;
+  // std::filesystem::path pathStart = basePath / baseName;
   uint32_t rolloverDiffSeconds = 0;
+  std::array<uint8_t, 524> filePathBuf{};
+  size_t basePathSize;
   std::array<uint8_t, MAX_FILESIZE> fileBuf{};
   timeval currentTv;
   timeval activeFileTv{};
@@ -106,8 +118,10 @@ class PersistentTmStore : public TmStoreFrontendSimpleIF, public SystemObject {
     uint32_t fromUnixTime = 0;
     uint32_t untilUnixTime = 0;
     uint32_t currentFileUnixStamp = 0;
-    std::filesystem::directory_iterator dirIter;
-    std::filesystem::directory_entry dirEntry;
+    std::filesystem::path currentFile;
+    std::set<DumpIndex> orderedDumpFilestamps{};
+    std::set<DumpIndex>::iterator dumpIter;
+    std::optional<uint8_t> currentSameFileIdx = 0;
     size_t fileSize = 0;
     size_t currentSize = 0;
   };
@@ -122,10 +136,13 @@ class PersistentTmStore : public TmStoreFrontendSimpleIF, public SystemObject {
   [[nodiscard]] MessageQueueId_t getCommandQueue() const override;
 
   void calcDiffSeconds(RolloverInterval intervalUnit, uint32_t intervalCount);
+  ReturnValue_t buildDumpSet(uint32_t fromUnixSeconds, uint32_t upToUnixSeconds);
   ReturnValue_t createMostRecentFile(std::optional<uint8_t> suffix);
   static ReturnValue_t pathToTime(const std::filesystem::path& path, struct tm& time);
   void fileToPackets(const std::filesystem::path& path, uint32_t unixStamp);
   ReturnValue_t loadNextDumpFile();
+  ReturnValue_t createFileName(timeval& tv, std::optional<uint8_t> suffix, size_t& fullPathLength);
+  std::optional<uint8_t> extractSuffix(const std::string& pathStr);
   bool updateBaseDir();
   ReturnValue_t assignAndOrCreateMostRecentFile();
 };

mission/tmtc/PusTmFunnel.cpp

@@ -1,5 +1,7 @@
 #include "PusTmFunnel.h"
 
+#include <limits>
+
 #include "eive/definitions.h"
 #include "eive/objects.h"
 #include "fsfw/ipc/CommandMessage.h"
@@ -11,8 +13,8 @@
 #include "tmtc/pusIds.h"
 
 PusTmFunnel::PusTmFunnel(TmFunnelBase::FunnelCfg cfg, StorageManagerIF &ramToFileStore,
-                         TimeReaderIF &timeReader, SdCardMountedIF &sdcMan)
-    : TmFunnelBase(cfg), ramToFileStore(ramToFileStore), timeReader(timeReader), sdcMan(sdcMan) {}
+                         TimeReaderIF &timeReader)
+    : TmFunnelBase(cfg), ramToFileStore(ramToFileStore), timeReader(timeReader) {}
 
 PusTmFunnel::~PusTmFunnel() = default;
 
@@ -21,6 +23,13 @@ ReturnValue_t PusTmFunnel::performOperation(uint8_t) {
   ReturnValue_t result;
   TmTcMessage currentMessage;
   unsigned int count = 0;
+  if (saveSequenceCount) {
+    result = saveSequenceCountToFile();
+    if (result != returnvalue::OK) {
+      sif::error << "PusTmFunnel: Storing sequence count to file has failed" << std::endl;
+    }
+    saveSequenceCount = false;
+  }
   result = tmQueue->receiveMessage(&currentMessage);
   while (result == returnvalue::OK) {
     result = handleTmPacket(currentMessage);
@@ -59,7 +68,33 @@ ReturnValue_t PusTmFunnel::handleTmPacket(TmTcMessage &message) {
     return result;
   }
   packet.setSequenceCount(sourceSequenceCount++);
+  // NOTE: This only works because the limit value bit width is below 16 bits!
   sourceSequenceCount = sourceSequenceCount % ccsds::LIMIT_SEQUENCE_COUNT;
+
+  // Message type counter handling.
+  uint8_t service = packet.getService();
+  bool insertionFailed = false;
+  auto mapIter = msgCounterMap.find(service);
+  if (mapIter == msgCounterMap.end()) {
+    auto iterPair = msgCounterMap.emplace(service, 0);
+    if (iterPair.second) {
+      mapIter = iterPair.first;
+    } else {
+      // Should really never never happen but you never know..
+      insertionFailed = true;
+    }
+  }
+  if (not insertionFailed) {
+    packet.setMessageCount(mapIter->second);
+    // Sane overflow handling.
+    if (mapIter->second == std::numeric_limits<uint16_t>::max()) {
+      mapIter->second = 0;
+    } else {
+      mapIter->second++;
+    }
+  }
+
+  // Re-calculate CRC after changing the fields. This operation HAS to come last!
   packet.updateErrorControl();
 
   // Send to persistent TM store if the packet matches some filter.

mission/tmtc/PusTmFunnel.h

@@ -9,6 +9,8 @@
 #include <mission/tmtc/PusTmRouteByFilterHelper.h>
 #include <mission/tmtc/TmFunnelBase.h>
 
+#include <atomic>
+#include <map>
 #include <vector>
 
 #include "PersistentTmStore.h"
@@ -25,7 +27,7 @@
 class PusTmFunnel : public TmFunnelBase {
  public:
   PusTmFunnel(TmFunnelBase::FunnelCfg cfg, StorageManagerIF &ramToFileStore,
-              TimeReaderIF &timeReader, SdCardMountedIF &sdcMan);
+              TimeReaderIF &timeReader);
   [[nodiscard]] const char *getName() const override;
   ~PusTmFunnel() override;
 
@@ -36,11 +38,10 @@ class PusTmFunnel : public TmFunnelBase {
   // Update TV stamp every 5 minutes
   static constexpr dur_millis_t TV_UPDATE_INTERVAL_SECS = 60 * 5;
 
-  uint16_t sourceSequenceCount = 0;
+  std::map<uint8_t, uint16_t> msgCounterMap;
   StorageManagerIF &ramToFileStore;
   TimeReaderIF &timeReader;
   bool storesInitialized = false;
-  SdCardMountedIF &sdcMan;
   PusTmRouteByFilterHelper persistentTmMap;
 
   ReturnValue_t handleTmPacket(TmTcMessage &message);

mission/tmtc/TmFunnelBase.cpp

@@ -2,6 +2,10 @@
 
 #include <fsfw/tmtcservices/TmTcMessage.h>
 
+#include <atomic>
+#include <filesystem>
+#include <fstream>
+
 #include "fsfw/ipc/QueueFactory.h"
 
 TmFunnelBase::TmFunnelBase(FunnelCfg cfg)
@@ -10,7 +14,10 @@ TmFunnelBase::TmFunnelBase(FunnelCfg cfg)
       tmStore(cfg.tmStore),
       ipcStore(cfg.ipcStore),
       tmQueue(QueueFactory::instance()->createMessageQueue(cfg.tmMsgDepth)),
-      liveDemux(*tmQueue) {}
+      liveDemux(*tmQueue),
+      sdcMan(cfg.sdcMan),
+      sequenceCounterFilename(cfg.sequenceCounterFilename),
+      saveSequenceCount(cfg.saveSequenceCount) {}
 
 ReturnValue_t TmFunnelBase::demultiplexLivePackets(store_address_t origStoreId,
                                                    const uint8_t *tmData, size_t tmSize) {
@@ -27,3 +34,38 @@ void TmFunnelBase::addLiveDestination(const char *name,
                                       const AcceptsTelemetryIF &downlinkDestination, uint8_t vcid) {
   liveDemux.addDestination(name, downlinkDestination, vcid);
 }
+
+ReturnValue_t TmFunnelBase::initialize() {
+  using namespace std::filesystem;
+  // The filesystem should always be available at the start.. Let's assume it always is, otherwise
+  // we just live with a regular 0 initialization. It simplifies a lot.
+  std::error_code e;
+  path filePath =
+      path(path(sdcMan.getCurrentMountPrefix()) / path("conf") / path(sequenceCounterFilename));
+  if (exists(filePath, e)) {
+    std::ifstream ifile(filePath);
+    if (ifile.bad()) {
+      sif::error << "TmFunnelBase::initialize: Faulty file open for sequence counter initialization"
+                 << std::endl;
+      return returnvalue::OK;
+    }
+    if (not(ifile >> sourceSequenceCount)) {
+      // Initialize to 0 in any case if reading a number failed.
+      sourceSequenceCount = 0;
+    }
+  }
+  return returnvalue::OK;
+}
+
+ReturnValue_t TmFunnelBase::saveSequenceCountToFile() {
+  using namespace std::filesystem;
+  std::error_code e;
+  path filePath =
+      path(path(sdcMan.getCurrentMountPrefix()) / path("conf") / path(sequenceCounterFilename));
+  std::ofstream ofile(filePath);
+  if (ofile.bad()) {
+    return returnvalue::FAILED;
+  }
+  ofile << sourceSequenceCount << "\n";
+  return returnvalue::OK;
+}

mission/tmtc/TmFunnelBase.h

@@ -6,25 +6,34 @@
 #include <fsfw/tmstorage/TmStoreFrontendSimpleIF.h>
 #include <fsfw/tmtcservices/AcceptsTelemetryIF.h>
 #include <fsfw/tmtcservices/TmTcMessage.h>
+#include <mission/memory/SdCardMountedIF.h>
 #include <mission/tmtc/PusLiveDemux.h>
 
+#include <atomic>
 #include <vector>
 
 class TmFunnelBase : public AcceptsTelemetryIF, public SystemObject {
  public:
   struct FunnelCfg {
     FunnelCfg(object_id_t objId, const char* name, StorageManagerIF& tmStore,
-              StorageManagerIF& ipcStore, uint32_t tmMsgDepth)
+              StorageManagerIF& ipcStore, uint32_t tmMsgDepth, SdCardMountedIF& sdcMan,
+              const char* sequenceCounterFilename, std::atomic_bool& saveSequenceCount)
         : objectId(objId),
           name(name),
           tmStore(tmStore),
           ipcStore(ipcStore),
-          tmMsgDepth(tmMsgDepth) {}
+          tmMsgDepth(tmMsgDepth),
+          sdcMan(sdcMan),
+          sequenceCounterFilename(sequenceCounterFilename),
+          saveSequenceCount(saveSequenceCount) {}
     object_id_t objectId;
     const char* name;
     StorageManagerIF& tmStore;
     StorageManagerIF& ipcStore;
     uint32_t tmMsgDepth;
+    SdCardMountedIF& sdcMan;
+    const char* sequenceCounterFilename;
+    std::atomic_bool& saveSequenceCount;
   };
   explicit TmFunnelBase(FunnelCfg cfg);
   [[nodiscard]] MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel) const override;
@@ -32,6 +41,9 @@ class TmFunnelBase : public AcceptsTelemetryIF, public SystemObject {
                           uint8_t vcid = 0);
   ReturnValue_t demultiplexLivePackets(store_address_t origStoreId, const uint8_t* tmData,
                                        size_t tmSize);
+  ReturnValue_t initialize() override;
+
+  ReturnValue_t saveSequenceCountToFile();
 
   ~TmFunnelBase() override;
 
@@ -41,6 +53,10 @@ class TmFunnelBase : public AcceptsTelemetryIF, public SystemObject {
   StorageManagerIF& ipcStore;
   MessageQueueIF* tmQueue = nullptr;
   PusLiveDemux liveDemux;
+  SdCardMountedIF& sdcMan;
+  const char* sequenceCounterFilename;
+  std::atomic_bool& saveSequenceCount;
+  uint16_t sourceSequenceCount = 0;
 };
 
 #endif /* MISSION_TMTC_TMFUNNELBASE_H_ */

unittest/CMakeLists.txt

@@ -4,7 +4,7 @@ add_subdirectory(mocks)
 target_sources(${UNITTEST_NAME} PRIVATE
     main.cpp
     testEnvironment.cpp
-    testStampInFilename.cpp
+    testGenericFilesystem.cpp
     hdlcEncodingRw.cpp
     printChar.cpp
 )

unittest/testEnvironment.cpp

@@ -27,7 +27,7 @@ void factory(void* args) {
   new HouseKeepingMock();
   eventManager = new EventManagerMock();
   new HealthTable(objects::HEALTH_TABLE);
-  new InternalErrorReporter(objects::INTERNAL_ERROR_REPORTER);
+  new InternalErrorReporter(objects::INTERNAL_ERROR_REPORTER, 5, false, 60.0);
   new CdsShortTimeStamper(objects::TIME_STAMPER);
 
   {

unittest/testGenericFilesystem.cpp

@@ -0,0 +1,43 @@
+
+#include <catch2/catch_test_macros.hpp>
+#include <cinttypes>
+
+#include "fsfw/timemanager/Clock.h"
+
+uint8_t extractSuffix(const std::string& pathStr) {
+  std::string numberStr;
+  // Find the position of the dot at the end of the file path
+  size_t dotPos = pathStr.find_last_of('.');
+  if (dotPos != std::string::npos && dotPos < pathStr.length() - 1) {
+    // Extract the substring after the dot
+    numberStr = pathStr.substr(dotPos + 1);
+  }
+  int number = std::stoi(numberStr);
+  if (number < 0 or number > std::numeric_limits<uint8_t>::max()) {
+    return 0;
+  }
+  return static_cast<uint8_t>(number);
+}
+
+TEST_CASE("Stamp in Filename", "[Stamp In Filename]") {
+  Clock::TimeOfDay_t tod;
+  std::string baseName = "verif";
+  std::string pathStr = "verif_2022-05-25T16:55:23Z.bin";
+  unsigned int underscorePos = pathStr.find_last_of('_');
+  std::string stampStr = pathStr.substr(underscorePos + 1);
+  float seconds = 0.0;
+  char* prefix = nullptr;
+  int count =
+      sscanf(stampStr.c_str(),
+             "%4" SCNu32 "-%2" SCNu32 "-%2" SCNu32 "T%2" SCNu32 ":%2" SCNu32 ":%2" SCNu32 "Z",
+             &tod.year, &tod.month, &tod.day, &tod.hour, &tod.minute, &tod.second);
+  static_cast<void>(count);
+  CHECK(count == 6);
+}
+
+TEST_CASE("Suffix Extraction") {
+  std::string pathStr = "/mnt/sd0/tm/hk/hk-some-stamp.bin.0";
+  CHECK(extractSuffix(pathStr) == 0);
+  pathStr = "/mnt/sd0/tm/hk/hk-some-stamp.bin.2";
+  CHECK(extractSuffix(pathStr) == 2);
+}

unittest/testStampInFilename.cpp

@@ -1,21 +0,0 @@
-
-#include <catch2/catch_test_macros.hpp>
-#include <cinttypes>
-
-#include "fsfw/timemanager/Clock.h"
-
-TEST_CASE("Stamp in Filename", "[Stamp In Filename]") {
-  Clock::TimeOfDay_t tod;
-  std::string baseName = "verif";
-  std::string pathStr = "verif_2022-05-25T16:55:23Z.bin";
-  unsigned int underscorePos = pathStr.find_last_of('_');
-  std::string stampStr = pathStr.substr(underscorePos + 1);
-  float seconds = 0.0;
-  char* prefix = nullptr;
-  int count =
-      sscanf(stampStr.c_str(),
-             "%4" SCNu32 "-%2" SCNu32 "-%2" SCNu32 "T%2" SCNu32 ":%2" SCNu32 ":%2" SCNu32 "Z",
-             &tod.year, &tod.month, &tod.day, &tod.hour, &tod.minute, &tod.second);
-  static_cast<void>(count);
-  CHECK(count == 6);
-}