diff --git a/Core/Src/stm32h7xx_it.c b/Core/Src/stm32h7xx_it.c
index 82a4496..4a075a3 100644
--- a/Core/Src/stm32h7xx_it.c
+++ b/Core/Src/stm32h7xx_it.c
@@ -114,7 +114,13 @@ void NMI_Handler(void)
 void HardFault_Handler(void)
 {
   /* USER CODE BEGIN HardFault_IRQn 0 */
-
+  __asm volatile (
+	"TST lr, #4 \n"
+	"ITE EQ \n"
+	"MRSEQ r0, MSP \n"
+	"MRSNE r0, PSP \n"
+	"B hard_fault_handler_c \n"
+  );
   /* USER CODE END HardFault_IRQn 0 */
   while (1)
   {
@@ -129,7 +135,13 @@ void HardFault_Handler(void)
 void MemManage_Handler(void)
 {
   /* USER CODE BEGIN MemoryManagement_IRQn 0 */
-
+  __asm volatile (
+	"TST lr, #4 \n"
+	"ITE EQ \n"
+	"MRSEQ r0, MSP \n"
+	"MRSNE r0, PSP \n"
+	"B hard_fault_handler_c \n"
+  );
   /* USER CODE END MemoryManagement_IRQn 0 */
   while (1)
   {