diff --git a/include/debug.h b/include/debug.h
new file mode 100644
index 0000000..c06e729
--- /dev/null
+++ b/include/debug.h
@@ -0,0 +1,46 @@
+#pragma once
+
+#include "Arduino.h"
+
+struct FaultStatusRegisters {
+  uint32_t HFSR;
+  uint32_t CFSR;
+  uint32_t BFAR;
+  uint32_t DFSR;
+  uint32_t AFSR;
+  uint32_t SHCSR;
+};
+
+struct FlashDumpInfo {
+  uint8_t _zero; // Used to check that the storage area has been initialized
+  uint32_t n_dumps;
+};
+
+struct FlashDump {
+  uint32_t stacked_registers[8];
+  FaultStatusRegisters fsr;
+};
+
+constexpr uint32_t FLASH_DUMP_ADDR_BASE = 0;
+constexpr uint32_t FLASH_DUMP_ADDR_INFO = FLASH_DUMP_ADDR_BASE + 0;
+constexpr uint32_t FLASH_DUMP_ADDR_DUMPS =
+    FLASH_DUMP_ADDR_INFO + sizeof(FlashDump);
+
+void flash_dump_write_fault(const uint32_t *stack,
+                            const FaultStatusRegisters *fsr);
+/**
+ * Read the FlashDumpInfo (and create it if it hasn't been initialized yet).
+ */
+const FlashDumpInfo *flash_dump_get_info();
+const FlashDump *flash_dump_get_fault(uint32_t n);
+
+bool check_serial_available();
+
+void print_dumped_faults();
+void print_stacked_registers(const uint32_t *stack);
+void print_fault_registers(const FaultStatusRegisters *fsr);
+
+FaultStatusRegisters get_current_fsr();
+
+void fault_handler(uint32_t *stack_addr, const char *fault_type,
+                   const int *leds, unsigned n_leds);
diff --git a/platformio.ini b/platformio.ini
index 0e736b7..37a40e3 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -12,6 +12,7 @@
 platform = atmelsam
 board = due
 framework = arduino
+lib_deps = sebnil/DueFlashStorage@^1.0.0
 
 [env:combustion]
 src_filter = +<*> -<18estw.cpp>
diff --git a/src/18stw.cpp b/src/18stw.cpp
index fb56a1d..83fa4d4 100644
--- a/src/18stw.cpp
+++ b/src/18stw.cpp
@@ -8,9 +8,11 @@
 #include <RotaryEncoder.h>
 #include <due_can.h>
 
+#include "debug.h"
+
 void setup() {
   set_pins();
-  // Serial.begin(9600);
+  Serial.begin(9600);
   // Serial.println("Hi");
   Init_Can_0();
   init_display();
@@ -22,4 +24,8 @@ void loop() {
   // Send_0x110();   //alle 10 ms als interrupt aufgerufen
   update_LED();
   update_display();
+
+  if (Serial.available() > 0 && Serial.read() == 'd') {
+    print_dumped_faults();
+  }
 }
\ No newline at end of file
diff --git a/src/debug.cpp b/src/debug.cpp
new file mode 100644
index 0000000..49dc785
--- /dev/null
+++ b/src/debug.cpp
@@ -0,0 +1,220 @@
+#include "debug.h"
+
+#include "FT18_STW_INIT.h"
+
+#include <DueFlashStorage.h>
+
+DueFlashStorage Flash;
+
+void flash_dump_fault(const uint32_t *stack, const FaultStatusRegisters *fsr) {
+  FlashDumpInfo info;
+  memcpy(&info, flash_dump_get_info(), sizeof(info));
+
+  FlashDump dump;
+  memcpy(dump.stacked_registers, stack, sizeof(dump.stacked_registers));
+  memcpy(&dump.fsr, fsr, sizeof(dump.fsr));
+
+  uint32_t new_addr = FLASH_DUMP_ADDR_DUMPS + info.n_dumps * sizeof(dump);
+  Flash.write(new_addr, (uint8_t *)&dump, sizeof(dump));
+
+  info.n_dumps++;
+  Flash.write(FLASH_DUMP_ADDR_INFO, (uint8_t *)&info, sizeof(info));
+}
+
+const FlashDumpInfo *flash_dump_get_info() {
+  if (Flash.read(FLASH_DUMP_ADDR_INFO) != 0) {
+    FlashDumpInfo info;
+    info._zero = 0;
+    info.n_dumps = 0;
+    Flash.write(FLASH_DUMP_ADDR_INFO, (uint8_t *)&info, sizeof(info));
+  }
+  return (const FlashDumpInfo *)Flash.readAddress(FLASH_DUMP_ADDR_INFO);
+}
+
+const FlashDump *flash_dump_get_fault(uint32_t n) {
+  uint32_t addr = FLASH_DUMP_ADDR_DUMPS + n * sizeof(FlashDump);
+  return (const FlashDump *)Flash.readAddress(addr);
+}
+
+bool check_serial_available() {
+  int available = Serial.availableForWrite();
+  if (available == 0) {
+    // No bytes available for write, maybe we were just writing? Wait 10 ms and
+    // check again.
+    delay(10);
+    available = Serial.availableForWrite();
+    if (available == 0) {
+      // Still no bytes available for write, looks like we can't write at all.
+      return false;
+    }
+  }
+  // Write a byte, and wait 10 ms. If the number of available bytes decreased,
+  // we can't write.
+  Serial.write('\0');
+  delay(10);
+  return Serial.availableForWrite() >= available;
+}
+
+void print_dumped_faults() {
+  uint32_t n = flash_dump_get_info()->n_dumps;
+
+  Serial.println("====================");
+  Serial.print(n);
+  Serial.println(" DUMPED FAULTS");
+
+  for (uint32_t i = 0; i < n; i++) {
+    Serial.println("--------------------");
+    Serial.print("Fault ");
+    Serial.println(i);
+    const FlashDump *dump = flash_dump_get_fault(i);
+    print_stacked_registers(dump->stacked_registers);
+    print_fault_registers(&dump->fsr);
+  }
+  Serial.println("====================");
+}
+
+void print_stacked_registers(const uint32_t *stack) {
+  Serial.print("R0    = ");
+  Serial.println(stack[0], HEX);
+  Serial.print("R1    = ");
+  Serial.println(stack[1], HEX);
+  Serial.print("R2    = ");
+  Serial.println(stack[2], HEX);
+  Serial.print("R3    = ");
+  Serial.println(stack[3], HEX);
+  Serial.print("R12   = ");
+  Serial.println(stack[4], HEX);
+  Serial.print("LR    = ");
+  Serial.println(stack[5], HEX);
+  Serial.print("PC    = ");
+  Serial.println(stack[6], HEX);
+  Serial.print("PSR   = ");
+  Serial.println(stack[7], HEX);
+  Serial.println();
+}
+
+void print_fault_registers(const FaultStatusRegisters *fsr) {
+  Serial.print("HFSR  = ");
+  Serial.println(fsr->HFSR, HEX);
+  Serial.print("CFSR  = ");
+  Serial.println(fsr->CFSR, HEX);
+  Serial.print("BFAR  = ");
+  Serial.println(fsr->BFAR, HEX);
+  Serial.print("DFSR  = ");
+  Serial.println(fsr->DFSR, HEX);
+  Serial.print("AFSR  = ");
+  Serial.println(fsr->AFSR, HEX);
+  Serial.print("SHCSR = ");
+  Serial.println(fsr->SHCSR, HEX);
+  Serial.println();
+}
+
+FaultStatusRegisters get_current_fsr() {
+  FaultStatusRegisters fsr;
+  fsr.HFSR = SCB->HFSR;
+  fsr.CFSR = SCB->CFSR;
+  fsr.BFAR = SCB->BFAR;
+  fsr.DFSR = SCB->DFSR;
+  fsr.AFSR = SCB->AFSR;
+  fsr.SHCSR = SCB->SHCSR;
+  return fsr;
+}
+
+void fault_handler(uint32_t *stack_addr, const char *fault_type,
+                   const int *leds, unsigned n_leds) {
+  Serial.begin(9600);
+  for (unsigned i = 0; i < n_leds; i++) {
+    pinMode(leds[i], OUTPUT);
+  }
+
+  FaultStatusRegisters fsr = get_current_fsr();
+
+  flash_dump_fault(stack_addr, &fsr);
+
+  while (1) {
+    if (check_serial_available()) {
+      Serial.println("====================");
+      Serial.println(fault_type);
+      Serial.println("====================");
+
+      print_stacked_registers(stack_addr);
+      print_fault_registers(&fsr);
+    }
+    for (unsigned i = 0; i < n_leds; i++) {
+      digitalWrite(leds[i], HIGH);
+    }
+    delay(500);
+    for (unsigned i = 0; i < n_leds; i++) {
+      digitalWrite(leds[i], LOW);
+    }
+    delay(500);
+  }
+}
+
+// If we don't wrap the handler functions in an extern C block, their names get
+// mangled and they won't override the weakly-linked handler functions defined
+// in the Arduino(-SAM) framework.
+extern "C" {
+
+// Get the stack pointer and pass it to the respective C handler. Depending on
+// the EXC_RETURN value, we need to either use the Main Stack Pointer (msp) or
+// the Process Stack Pointer (psp).
+void HardFault_Handler(void) {
+  __ASM("tst lr, #4\n"
+        "ite eq\n"
+        "mrseq r0, msp\n"
+        "mrsne r0, psp\n"
+        "b HardFault_Handler_C");
+}
+
+void MemManage_Handler(void) {
+  __ASM("tst lr, #4\n"
+        "ite eq\n"
+        "mrseq r0, msp\n"
+        "mrsne r0, psp\n"
+        "b MemManage_Handler_C");
+}
+
+void BusFault_Handler(void) {
+  __ASM("tst lr, #4\n"
+        "ite eq\n"
+        "mrseq r0, msp\n"
+        "mrsne r0, psp\n"
+        "b BusFault_Handler_C");
+}
+
+void UsageFault_Handler(void) {
+  __ASM("tst lr, #4\n"
+        "ite eq\n"
+        "mrseq r0, msp\n"
+        "mrsne r0, psp\n"
+        "b UsageFault_Handler_C");
+}
+
+void HardFault_Handler_C(uint32_t *stack_addr) {
+  // All LEDs
+  static const int leds[] = {led1,  led2,  led3,  led4,  led5,  led6,
+                             led7,  led8,  led9,  led10, led11, led12,
+                             led13, led14, led15, led16};
+  fault_handler(stack_addr, "HARD FAULT", leds, sizeof(leds) / sizeof(int));
+}
+
+void MemManage_Handler_C(uint32_t *stack_addr) {
+  // Left of display
+  static const int leds[] = {led11, led12, led13};
+  fault_handler(stack_addr, "MEMMANAGE FAULT", leds,
+                sizeof(leds) / sizeof(int));
+}
+
+void BusFault_Handler_C(uint32_t *stack_addr) {
+  // Right of display
+  static const int leds[] = {led14, led15, led16};
+  fault_handler(stack_addr, "BUS FAULT", leds, sizeof(leds) / sizeof(int));
+}
+
+void UsageFault_Handler_C(uint32_t *stack_addr) {
+  // Left and right of display
+  static const int leds[] = {led11, led12, led13, led14, led15, led16};
+  fault_handler(stack_addr, "USAGE FAULT", leds, sizeof(leds) / sizeof(int));
+}
+}