diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c
index 01b54730cc..3102f4a0bf 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c
@@ -58,3 +58,22 @@ void dma_flush(const void *buf, uint32_t size)
     }
 #endif
 }
+
+#if CH_DBG_ENABLE_STACK_CHECK == TRUE
+void show_stack_usage(void)
+{
+  thread_t *tp;
+
+  tp = chRegFirstThread();
+  do {
+      uint32_t stklimit = (uint32_t)tp->wabase;
+      uint8_t *p = (uint8_t *)tp->wabase;
+      while (*p == CH_DBG_STACK_FILL_VALUE) {
+          p++;
+      }
+      uint32_t stack_left = ((uint32_t)p) - stklimit;
+      printf("%s %u\n", tp->name, stack_left);
+      tp = chRegNextThread(tp);
+  } while (tp != NULL);
+}
+#endif
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h
index bdff926b3e..3a760fdde3 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h
@@ -32,6 +32,11 @@ void dma_invalidate(void *buf, uint32_t size);
  */
 void dma_flush(const void *buf, uint32_t size);
 
+#if CH_DBG_ENABLE_STACK_CHECK == TRUE
+// print stack usage
+void show_stack_usage(void);
+#endif
+    
 #ifdef __cplusplus
 }
 #endif