mission/com/LiveTmTask.cpp

@@ -9,6 +9,7 @@
 
 static constexpr bool DEBUG_TM_QUEUE_SPEED = false;
 std::atomic_bool signals::CFDP_CHANNEL_THROTTLE_SIGNAL = false;
+std::atomic_uint32_t signals::CFDP_MSG_COUNTER = 0;
 
 LiveTmTask::LiveTmTask(object_id_t objectId, PusTmFunnel& pusFunnel, CfdpTmFunnel& cfdpFunnel,
                        VirtualChannel& channel, const std::atomic_bool& ptmeLocked,
@@ -52,16 +53,14 @@ ReturnValue_t LiveTmTask::performOperation(uint8_t opCode) {
           }
         }
       }
-    } else {
-      consecutiveNoBlockWriteCounter = 0;
     }
     if (channel.isBusy() and !throttlePeriodOngoing) {
       // Throttle CFDP packet creator. It is by far the most relevant data creator, so throttling
       // it is the easiest way to handle back pressure for now in a sensible way.
       throttleCfdp();
     } else if (!channel.isBusy() and throttlePeriodOngoing) {
-      if(minimumPeriodThrottleCd.hasTimedOut() and consecutiveNoBlockWriteCounter >= 10) {
-        sif::debug << "releasing cfdp" << std::endl;
+      // Half full/empty flow control: Release the CFDP is the queue is empty enough.
+      if (signals::CFDP_MSG_COUNTER <= config::LIVE_CHANNEL_CFDP_QUEUE_SIZE / 2) {
         releaseCfdp();
       }
     }
@@ -144,16 +143,21 @@ void LiveTmTask::readCommandQueue(void) {
   }
 }
 
-ReturnValue_t LiveTmTask::handleRegularTmQueue() { return handleGenericTmQueue(*regularTmQueue); }
+ReturnValue_t LiveTmTask::handleRegularTmQueue() {
+  return handleGenericTmQueue(*regularTmQueue, false);
+}
 
-ReturnValue_t LiveTmTask::handleCfdpTmQueue() { return handleGenericTmQueue(*cfdpTmQueue); }
+ReturnValue_t LiveTmTask::handleCfdpTmQueue() { return handleGenericTmQueue(*cfdpTmQueue, true); }
 
-ReturnValue_t LiveTmTask::handleGenericTmQueue(MessageQueueIF& queue) {
+ReturnValue_t LiveTmTask::handleGenericTmQueue(MessageQueueIF& queue, bool isCfdp) {
   TmTcMessage message;
   ReturnValue_t result = queue.receiveMessage(&message);
   if (result == MessageQueueIF::EMPTY) {
     return result;
   }
+  if (signals::CFDP_MSG_COUNTER > 0) {
+    signals::CFDP_MSG_COUNTER--;
+  }
   store_address_t storeId = message.getStorageId();
   const uint8_t* data = nullptr;
   size_t size = 0;
@@ -165,26 +169,19 @@ ReturnValue_t LiveTmTask::handleGenericTmQueue(MessageQueueIF& queue) {
     return result;
   }
 
-  if(ptmeLocked) {
-    consecutiveNoBlockWriteCounter= 0;
-  }
   if (!ptmeLocked) {
     size_t partiallyWrittenSize = 0;
     result = channel.write(data, size, partiallyWrittenSize);
     if (result == DirectTmSinkIF::PARTIALLY_WRITTEN) {
-      consecutiveNoBlockWriteCounter = 0;
       // Already throttle CFDP.
       throttleCfdp();
       result = channel.handleLastWriteSynchronously(data, size, partiallyWrittenSize, 200);
       if (result != returnvalue::OK) {
         // TODO: Event? Might lead to dangerous spam though..
-        sif::warning
-            << "LiveTmTask: Synchronous write of last segment failed with code 0x"
+        sif::warning << "LiveTmTask: Synchronous write of last segment failed with code 0x"
                      << std::setw(4) << std::hex << result << std::dec << std::endl;
       }
-      minimumPeriodThrottleCd.resetTimer();
-    } else {
-      consecutiveNoBlockWriteCounter++;
+      // minimumPeriodThrottleCd.resetTimer();
     }
   }
   // Try delete in any case, ignore failures (which should not happen), it is more important to
@@ -195,7 +192,7 @@ ReturnValue_t LiveTmTask::handleGenericTmQueue(MessageQueueIF& queue) {
 
 void LiveTmTask::throttleCfdp() {
   throttlePeriodOngoing = true;
-  minimumPeriodThrottleCd.resetTimer();
+  // minimumPeriodThrottleCd.resetTimer();
   signals::CFDP_CHANNEL_THROTTLE_SIGNAL = true;
 }
 

mission/com/LiveTmTask.h

@@ -10,7 +10,9 @@
 #include <mission/com/VirtualChannelWithQueue.h>
 #include <mission/tmtc/CfdpTmFunnel.h>
 #include <mission/tmtc/PusTmFunnel.h>
+
 #include <cstdint>
+
 #include "eive/definitions.h"
 
 class LiveTmTask : public SystemObject,
@@ -37,21 +39,20 @@ class LiveTmTask : public SystemObject,
   ModeHelper modeHelper;
   Mode_t mode = HasModesIF::MODE_OFF;
   Countdown tmFunnelCd = Countdown(100);
-  uint32_t consecutiveNoBlockWriteCounter = 0;
   PusTmFunnel& pusFunnel;
   CfdpTmFunnel& cfdpFunnel;
   VirtualChannel& channel;
   const std::atomic_bool& ptmeLocked;
   // This countdown ensures that the CFDP is always throttled with a minimum period. Only after
   // this period, the CFDP can be released if the channel is not busy.
-  Countdown minimumPeriodThrottleCd = Countdown(config::CFDP_THROTTLE_PERIOD_MS);
+  // Countdown minimumPeriodThrottleCd = Countdown(config::CFDP_THROTTLE_PERIOD_MS);
   bool throttlePeriodOngoing = false;
 
   void readCommandQueue(void);
 
   ReturnValue_t handleRegularTmQueue();
   ReturnValue_t handleCfdpTmQueue();
-  ReturnValue_t handleGenericTmQueue(MessageQueueIF& queue);
+  ReturnValue_t handleGenericTmQueue(MessageQueueIF& queue, bool isCfdp);
 
   MessageQueueId_t getCommandQueue() const override;
 

mission/sysDefs.h

@@ -15,6 +15,7 @@ namespace signals {
 
 extern std::atomic_bool CFDP_CHANNEL_THROTTLE_SIGNAL;
 extern std::atomic_uint16_t I2C_FATAL_ERRORS;
+extern std::atomic_uint32_t CFDP_MSG_COUNTER;
 
 }  // namespace signals