From 80160e829166c48957b1747df69c98ad649c2a51 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sun, 25 Jun 2023 13:07:31 +0200
Subject: [PATCH] that was hopefully the last set of fixes

---
 mission/tmtc/PusTmFunnel.cpp | 29 ++++++++++++++++++++++-------
 1 file changed, 22 insertions(+), 7 deletions(-)

diff --git a/mission/tmtc/PusTmFunnel.cpp b/mission/tmtc/PusTmFunnel.cpp
index e5285ba5..87648e55 100644
--- a/mission/tmtc/PusTmFunnel.cpp
+++ b/mission/tmtc/PusTmFunnel.cpp
@@ -68,18 +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;
-  uint8_t service = packet.getMessageTypeCounter();
+
+  // Message type counter handling.
+  uint8_t service = packet.getService();
+  bool insertionFailed = false;
   auto mapIter = msgCounterMap.find(service);
   if (mapIter == msgCounterMap.end()) {
-    msgCounterMap.emplace(service, 0);
+    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 (mapIter->second == std::numeric_limits<uint16_t>::max()) {
-    mapIter->second = 0;
-  } else {
-    mapIter->second++;
+  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++;
+    }
   }
-  packet.setMessageCount(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.