diff --git a/src/fsfw/subsystem/SubsystemBase.cpp b/src/fsfw/subsystem/SubsystemBase.cpp
index a876ae33..eccd447c 100644
--- a/src/fsfw/subsystem/SubsystemBase.cpp
+++ b/src/fsfw/subsystem/SubsystemBase.cpp
@@ -24,25 +24,6 @@ ReturnValue_t SubsystemBase::checkStateAgainstTable(HybridIterator<ModeListEntry
   using namespace mode;
   std::map<object_id_t, ChildInfo>::iterator childIter;
 
-  auto checkSubmode = [&]() {
-    uint8_t mask;
-    bool submodesAllowedMask = tableIter.value->submodesAllowed(&mask);
-    uint8_t submodeToCheckAgainst = tableIter.value->getSubmode();
-    if (tableIter.value->inheritSubmode()) {
-      submodeToCheckAgainst = targetSubmode;
-    }
-    if (not submodesAllowedMask) {
-      if (childIter->second.submode != submodeToCheckAgainst) {
-        return returnvalue::FAILED;
-      }
-    }
-    if (submodesAllowedMask) {
-      if ((childIter->second.submode | mask) != mask) {
-        return returnvalue::FAILED;
-      }
-    }
-    return returnvalue::OK;
-  };
   for (; tableIter.value != NULL; ++tableIter) {
     object_id_t object = tableIter.value->getObject();
 
@@ -53,9 +34,22 @@ ReturnValue_t SubsystemBase::checkStateAgainstTable(HybridIterator<ModeListEntry
     if (childIter->second.mode != tableIter.value->getMode()) {
       return returnvalue::FAILED;
     }
-    ReturnValue_t result = checkSubmode();
-    if (result != returnvalue::OK) {
-      return result;
+
+    // Check submodes here.
+    uint8_t mask;
+    bool submodesAllowedMask = tableIter.value->submodesAllowed(&mask);
+    uint8_t submodeToCheckAgainst = tableIter.value->getSubmode();
+    if (tableIter.value->inheritSubmode()) {
+      submodeToCheckAgainst = targetSubmode;
+    }
+    if (submodesAllowedMask) {
+      if ((childIter->second.submode | mask) != mask) {
+        return returnvalue::FAILED;
+      }
+    } else {
+      if (childIter->second.submode != submodeToCheckAgainst) {
+        return returnvalue::FAILED;
+      }
     }
   }
   return returnvalue::OK;
diff --git a/src/fsfw/subsystem/modes/ModeDefinitions.h b/src/fsfw/subsystem/modes/ModeDefinitions.h
index b60f01de..70a66fa6 100644
--- a/src/fsfw/subsystem/modes/ModeDefinitions.h
+++ b/src/fsfw/subsystem/modes/ModeDefinitions.h
@@ -13,6 +13,8 @@ enum SpecialSubmodeFlags : uint8_t { INHERIT = 1 << 0, ALLOWED_MASK = 1 << 1 };
 class ModeListEntry : public SerialLinkedListAdapter<SerializeIF>,
                       public LinkedElement<ModeListEntry> {
  public:
+  static constexpr uint8_t ALL_SUBMODES_ALLOWED_MASK = 0xff;
+
   ModeListEntry() : SerialLinkedListAdapter(), LinkedElement<ModeListEntry>(this) { setLinks(); }
 
   SerializeElement<uint32_t> value1 = 0;
@@ -101,10 +103,13 @@ class ModeListEntry : public SerialLinkedListAdapter<SerializeIF>,
   /**
    * Specialization of @enableSubmodeAllowed which allows all submodes.
    */
-  void allowAllSubmodes() { enableSubmodeAllowed(0xff); }
+  void allowAllSubmodes() { enableSubmodeAllowed(ALL_SUBMODES_ALLOWED_MASK); }
 
   /**
-   * Enable an allowed submode mask for mode checks.
+   * Enable an allowed submode mask for mode checks. Any submode which contains bits
+   * outside of the mask will be declined.
+   *
+   * For example, for a mask of 0b11, only the modes 0b00, 0b01 and 0b11 will be accepted.
    */
   void enableSubmodeAllowed(uint8_t mask) {
     value4.entry |= mode::SpecialSubmodeFlags::ALLOWED_MASK;