diff --git a/src/fsfw/tmtcpacket/SpacePacket.h b/src/fsfw/tmtcpacket/SpacePacket.h
index 16673319..a092712c 100644
--- a/src/fsfw/tmtcpacket/SpacePacket.h
+++ b/src/fsfw/tmtcpacket/SpacePacket.h
@@ -73,7 +73,7 @@ namespace spacepacket {
 
 constexpr uint16_t getSpacePacketIdFromApid(bool isTc, uint16_t apid,
         bool secondaryHeaderFlag = true) {
-    return (((isTc << 5) & 0x10) | ((secondaryHeaderFlag << 4) & 0x08) |
+    return ((isTc << 4) | (secondaryHeaderFlag << 3) |
             ((apid >> 8) & 0x07)) << 8 | (apid & 0x00ff);
 }
 
diff --git a/tests/src/fsfw_tests/unit/tmtcpacket/CMakeLists.txt b/tests/src/fsfw_tests/unit/tmtcpacket/CMakeLists.txt
index 36838b24..958bda40 100644
--- a/tests/src/fsfw_tests/unit/tmtcpacket/CMakeLists.txt
+++ b/tests/src/fsfw_tests/unit/tmtcpacket/CMakeLists.txt
@@ -1,3 +1,3 @@
 target_sources(${FSFW_TEST_TGT} PRIVATE
-	PusTmTest.cpp
+	testCcsds.cpp
 )
diff --git a/tests/src/fsfw_tests/unit/tmtcpacket/testCcsds.cpp b/tests/src/fsfw_tests/unit/tmtcpacket/testCcsds.cpp
new file mode 100644
index 00000000..8f531805
--- /dev/null
+++ b/tests/src/fsfw_tests/unit/tmtcpacket/testCcsds.cpp
@@ -0,0 +1,11 @@
+#include <catch2/catch_test_macros.hpp>
+
+#include "fsfw/tmtcpacket/SpacePacket.h"
+
+TEST_CASE( "CCSDS Test" , "[ccsds]") {
+    REQUIRE(spacepacket::getTcSpacePacketIdFromApid(0x22) == 0x1822);
+    REQUIRE(spacepacket::getTmSpacePacketIdFromApid(0x22) == 0x0822);
+
+    REQUIRE(spacepacket::getTcSpacePacketIdFromApid(0x7ff) == 0x1fff);
+    REQUIRE(spacepacket::getTmSpacePacketIdFromApid(0x7ff) == 0xfff);
+}