mission/com/LiveTmTask.cpp

@@ -58,16 +58,9 @@ ReturnValue_t LiveTmTask::performOperation(uint8_t opCode) {
                      << result << std::dec << std::endl;
       }
     }
-    if (channelIsBusy and !throttlePeriodOngoing) {
+
-      // Throttle CFDP packet creator. It is by far the most relevant data creator, so throttling
+    cfdpBackpressureHandling();
-      // it is the easiest way to handle back pressure for now in a sensible way.
+
-      throttleCfdp();
-    } else if (!channelIsBusy and throttlePeriodOngoing) {
-      // 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();
-      }
-    }
     if (!handledTm) {
       if (tmFunnelCd.hasTimedOut()) {
         pusFunnel.performOperation(0);
@@ -159,9 +152,10 @@ ReturnValue_t LiveTmTask::handleGenericTmQueue(MessageQueueIF& queue, bool isCfd
   if (result == MessageQueueIF::EMPTY) {
     return result;
   }
-  if (signals::CFDP_MSG_COUNTER > 0) {
+  if (isCfdp and signals::CFDP_MSG_COUNTER > 0) {
     signals::CFDP_MSG_COUNTER--;
   }
+  sif::debug << "CFDP msg counter: " << signals::CFDP_MSG_COUNTER << std::endl;
   store_address_t storeId = message.getStorageId();
   const uint8_t* data = nullptr;
   size_t size = 0;
@@ -177,17 +171,12 @@ ReturnValue_t LiveTmTask::handleGenericTmQueue(MessageQueueIF& queue, bool isCfd
     size_t writtenSize = 0;
     result = channel.write(data, size, writtenSize);
     if (result == DirectTmSinkIF::PARTIALLY_WRITTEN) {
-      // Already throttle CFDP.
-      throttleCfdp();
       result = channel.handleWriteCompletionSynchronously(writtenSize, 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"
                      << std::setw(4) << std::hex << result << std::dec << std::endl;
       }
-      // This is a bit of a hack: If a partial write was performed and synchronously finished, we
-      // treat this like a busy channel.
-      channelIsBusy = true;
     }
   }
   // Try delete in any case, ignore failures (which should not happen), it is more important to
@@ -199,11 +188,13 @@ ReturnValue_t LiveTmTask::handleGenericTmQueue(MessageQueueIF& queue, bool isCfd
 void LiveTmTask::throttleCfdp() {
   throttlePeriodOngoing = true;
   signals::CFDP_CHANNEL_THROTTLE_SIGNAL = true;
+  sif::debug << "throttling CFDP" << std::endl;
 }
 
 void LiveTmTask::releaseCfdp() {
   throttlePeriodOngoing = false;
   signals::CFDP_CHANNEL_THROTTLE_SIGNAL = false;
+  sif::debug << "releasing CFDP" << std::endl;
 }
 
 void LiveTmTask::updateBusyFlag() {
@@ -211,6 +202,21 @@ void LiveTmTask::updateBusyFlag() {
   channelIsBusy = channel.isBusy();
 }
 
+void LiveTmTask::cfdpBackpressureHandling() {
+  if (channelIsBusy 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.
+    if (signals::CFDP_MSG_COUNTER >= (config::LIVE_CHANNEL_CFDP_QUEUE_SIZE / 2)) {
+      throttleCfdp();
+    }
+  } else if (!channelIsBusy and throttlePeriodOngoing) {
+    // 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 / 4)) {
+      releaseCfdp();
+    }
+  }
+}
+
 ModeTreeChildIF& LiveTmTask::getModeTreeChildIF() { return *this; }
 
 ReturnValue_t LiveTmTask::initialize() {

mission/com/LiveTmTask.h

@@ -56,6 +56,8 @@ class LiveTmTask : public SystemObject,
 
   void getMode(Mode_t* mode, Submode_t* submode) override;
 
+  void cfdpBackpressureHandling();
+
   ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode,
                                  uint32_t* msToReachTheMode) override;