diff --git a/AMS_Master_Code/Core/Inc/swo_log.h b/AMS_Master_Code/Core/Inc/swo_log.h
new file mode 100644
index 0000000..f097e5d
--- /dev/null
+++ b/AMS_Master_Code/Core/Inc/swo_log.h
@@ -0,0 +1,80 @@
+/*
+ * swo_log.h
+ *
+ * Created on: 20.01.2025
+ * Author: Kilian
+ *
+ * Use the SWO interface to log messages
+ */
+#pragma once
+#ifndef __SWO_LOG_H
+#define __SWO_LOG_H
+#include "stm32h7xx_hal.h"
+#include <stdarg.h>
+#include <stdio.h>
+
+enum log_level_t {
+    LOG_LEVEL_NOISY,
+    LOG_LEVEL_DEBUG,
+    LOG_LEVEL_INFO,
+    LOG_LEVEL_WARNING,
+    LOG_LEVEL_ERROR,
+    LOG_LEVEL_FATAL
+};
+
+[[maybe_unused]] static const char * log_level_names[] = {
+    "\033[37m[NOISY]\033[0m",
+    "\033[37m[DEBUG]\033[0m",
+    "\033[97m[INFO] \033[0m",
+    "\033[93m[WARN] \033[0m",
+    "\033[31m[ERROR]\033[0m",
+    "\033[31m[FATAL]\033[0m"
+};
+
+static inline bool __ITM_channel_enabled(uint32_t channel) {
+    return (ITM->TER & (1UL << channel)) != 0UL;
+}
+
+//adapted from ITM_SendChar() in the CMSIS-Core
+//
+//channel should be between 0 and 31
+__STATIC_INLINE uint32_t __swo_putc (uint32_t c, unsigned int channel) {
+  if (((ITM->TCR & ITM_TCR_ITMENA_Msk) != 0UL) &&      /* ITM enabled */
+      ((ITM->TER & (1UL << channel)  ) != 0UL)   )     /* ITM Port enabled */
+  {
+    while (ITM->PORT[channel].u32 == 0UL)
+    {
+      __NOP();
+    }
+    ITM->PORT[channel].u8 = (uint8_t)c;
+  }
+  return (c);
+}
+
+static inline void __swo_print(const char *str, unsigned int channel) {
+    if (!__ITM_channel_enabled(channel))
+    {
+        return;
+    }
+    while(*str)
+    {
+        __swo_putc(*str++, channel);
+    }
+}
+
+//Print a message to the SWO interface
+//
+//Each log level is printed to a different ITM channel
+static inline void debug_log(enum log_level_t level, const char *msg, ...) {
+    va_list args;
+    va_start(args, msg);
+    char buffer[256];
+    vsnprintf(buffer, sizeof(buffer), msg, args);
+    va_end(args);
+    __swo_print(log_level_names[level], level);
+    __swo_print(buffer, level);
+    __swo_putc('\n', level);
+}
+
+#endif /* __SWO_LOG_H */
+