diff --git a/Core/Src/stm32h7xx_it.c b/Core/Src/stm32h7xx_it.c
index 4c2c037..82a4496 100644
--- a/Core/Src/stm32h7xx_it.c
+++ b/Core/Src/stm32h7xx_it.c
@@ -159,7 +159,13 @@ void BusFault_Handler(void)
 void UsageFault_Handler(void)
 {
   /* USER CODE BEGIN UsageFault_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 UsageFault_IRQn 0 */
   while (1)
   {