From 2279eab5e764e73b81560a54827010279f962460 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Fri, 13 Oct 2023 14:20:50 +0200
Subject: [PATCH] maybe this works better?

---
 mission/com/LiveTmTask.cpp | 17 +++++++++++++----
 mission/com/LiveTmTask.h   |  2 ++
 2 files changed, 15 insertions(+), 4 deletions(-)

diff --git a/mission/com/LiveTmTask.cpp b/mission/com/LiveTmTask.cpp
index a723daa6..5b337030 100644
--- a/mission/com/LiveTmTask.cpp
+++ b/mission/com/LiveTmTask.cpp
@@ -37,7 +37,8 @@ ReturnValue_t LiveTmTask::performOperation(uint8_t opCode) {
     // TODO: Must read CFDP TM queue and regular TM queue and forward them. Handle regular queue
     //       first.
     handledTm = false;
-    if (!channel.isBusy()) {
+    updateBusyFlag();
+    if (!channelIsBusy) {
       result = handleRegularTmQueue();
       if (result == MessageQueueIF::EMPTY) {
         result = handleCfdpTmQueue();
@@ -54,14 +55,14 @@ ReturnValue_t LiveTmTask::performOperation(uint8_t opCode) {
         }
       }
     }
-    if (channel.isBusy()) {
+    if (channelIsBusy) {
       sif::debug << "busy" << std::endl;
     }
-    if (channel.isBusy() and !throttlePeriodOngoing) {
+    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.
       throttleCfdp();
-    } else if (!channel.isBusy() and throttlePeriodOngoing) {
+    } 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();
@@ -184,6 +185,9 @@ ReturnValue_t LiveTmTask::handleGenericTmQueue(MessageQueueIF& queue, bool isCfd
         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
@@ -204,6 +208,11 @@ void LiveTmTask::releaseCfdp() {
   sif::debug << "releasing CFDP" << std::endl;
 }
 
+void LiveTmTask::updateBusyFlag() {
+  // We cache this as a member, because the busy bit can toggle very quickly..
+  channelIsBusy = channel.isBusy();
+}
+
 ModeTreeChildIF& LiveTmTask::getModeTreeChildIF() { return *this; }
 
 ReturnValue_t LiveTmTask::initialize() {
diff --git a/mission/com/LiveTmTask.h b/mission/com/LiveTmTask.h
index 41b03ece..0727bad1 100644
--- a/mission/com/LiveTmTask.h
+++ b/mission/com/LiveTmTask.h
@@ -47,6 +47,7 @@ class LiveTmTask : public SystemObject,
   // this period, the CFDP can be released if the channel is not busy.
   // Countdown minimumPeriodThrottleCd = Countdown(config::CFDP_THROTTLE_PERIOD_MS);
   bool throttlePeriodOngoing = false;
+  bool channelIsBusy = false;
 
   void readCommandQueue(void);
 
@@ -66,6 +67,7 @@ class LiveTmTask : public SystemObject,
   void announceMode(bool recursive) override;
   void throttleCfdp();
   void releaseCfdp();
+  void updateBusyFlag();
 
   object_id_t getObjectId() const override;
   const HasHealthIF* getOptHealthIF() const override;