diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.cpp b/libraries/AP_HAL_ChibiOS/Scheduler.cpp
index b6a84ca571..7e277a317a 100644
--- a/libraries/AP_HAL_ChibiOS/Scheduler.cpp
+++ b/libraries/AP_HAL_ChibiOS/Scheduler.cpp
@@ -500,7 +500,7 @@ void Scheduler::_io_thread(void* arg)
 #if CH_DBG_ENABLE_STACK_CHECK == TRUE
         if (now - last_stack_check_ms > 1000) {
             last_stack_check_ms = now;
-            check_stack_free();
+            sched->check_stack_free();
         }
 #endif
     }
@@ -711,20 +711,29 @@ void Scheduler::watchdog_pat(void)
  */
 void Scheduler::check_stack_free(void)
 {
+    // we raise an internal error stack_overflow when the available
+    // stack on any thread or the ISR stack drops below this
+    // threshold. This means we get an overflow error when we haven't
+    // yet completely run out of stack. This gives us a good
+    // pre-warning when we are getting too close
+#if defined(STM32F1)
     const uint32_t min_stack = 32;
-    bool ok = false;
+#else
+    const uint32_t min_stack = 64;
+#endif
 
-    if (stack_free(__main_stack_base__) < min_stack) {
-        AP::internalerror().error(error_number, 0xFFFF);
+    if (stack_free(&__main_stack_base__) < min_stack) {
+        // use "line number" of 0xFFFF for ISR stack low
+        AP::internalerror().error(AP_InternalError::error_t::stack_overflow, 0xFFFF);
     }
 
-    thread_t *tp = chRegFirstThread();
-    do {
+    for (thread_t *tp = chRegFirstThread(); tp; tp = chRegNextThread(tp)) {
         if (stack_free(tp->wabase) < min_stack) {
-            AP::internalerror().error(error_number, tp->prio);
+            // use task priority for line number. This allows us to
+            // identify the task fairly reliably
+            AP::internalerror().error(AP_InternalError::error_t::stack_overflow, tp->prio);
         }
-        tp = chRegNextThread(tp);
-    } while (tp != NULL);
+    }
 }
 #endif // CH_DBG_ENABLE_STACK_CHECK == TRUE
 
diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp
index 7861c73762..bc2dc3baf9 100644
--- a/libraries/AP_HAL_ChibiOS/Util.cpp
+++ b/libraries/AP_HAL_ChibiOS/Util.cpp
@@ -300,12 +300,12 @@ bool Util::was_watchdog_reset() const
  */
 size_t Util::thread_info(char *buf, size_t bufsize)
 {
-  thread_t *tp;
   size_t total = 0;
 
   // a header to allow for machine parsers to determine format
-  int n = snprintf(buf, bufsize, "ThreadsV2\nISR PRI=255 sp=%p STACK_FREE=%u/%u\n",
-                   __main_stack_base__, stack_free(__main_stack_base__), __main_stack_size__);
+  const uint32_t isr_stack_size = uint32_t((const uint8_t *)&__main_stack_end__ - (const uint8_t *)&__main_stack_base__);
+  int n = snprintf(buf, bufsize, "ThreadsV2\nISR           PRI=255 sp=%p STACK=%u/%u\n",
+                   &__main_stack_base__, stack_free(&__main_stack_base__), isr_stack_size);
   if (n <= 0) {
       return 0;
   }
@@ -313,21 +313,28 @@ size_t Util::thread_info(char *buf, size_t bufsize)
   bufsize -= n;
   total += n;
 
-  tp = chRegFirstThread();
-
-  do {
-      const uint32_t total_stack = uint32_t(tp) - uint32_t(tp->wabase);
-      n = snprintf(buf, bufsize, "%-13.13s PRI=%3u sp=%p STACK_LEFT=%u/%u\n",
-                   tp->name, unsigned(tp->prio), tp->wabase,
-                   stack_free(tp->wabase), total_stack);
-      if (n <= 0) {
-          break;
+  for (thread_t *tp = chRegFirstThread(); tp; tp = chRegNextThread(tp)) {
+      uint32_t total_stack;
+      if (tp->wabase == (void*)&__main_thread_stack_base__) {
+          // main thread has its stack separated from the thread context
+          total_stack = uint32_t((const uint8_t *)&__main_thread_stack_end__ - (const uint8_t *)&__main_thread_stack_base__);
+      } else {
+          // all other threads have their thread context pointer
+          // above the stack top
+          total_stack = uint32_t(tp) - uint32_t(tp->wabase);
       }
-      buf += n;
-      bufsize -= n;
-      total += n;
-      tp = chRegNextThread(tp);
-  } while (tp != NULL);
+      if (bufsize > 0) {
+          n = snprintf(buf, bufsize, "%-13.13s PRI=%3u sp=%p STACK=%u/%u\n",
+                       tp->name, unsigned(tp->prio), tp->wabase,
+                       stack_free(tp->wabase), total_stack);
+          if (n > bufsize) {
+              n = bufsize;
+          }
+          buf += n;
+          bufsize -= n;
+          total += n;
+      }
+  }
 
   return total;
 }
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c
index 9b76c86c31..44997c02b4 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c
@@ -462,6 +462,6 @@ uint32_t stack_free(void *stack_base)
     while (*p == canary_word) {
         p++;
     }
-    return uint32_t(p) - uint32_t(stack_base);
+    return ((uint32_t)p) - (uint32_t)stack_base;
 }
 #endif
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h
index 3b38407759..ae68587e52 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h
@@ -117,8 +117,10 @@ void stack_overflow(thread_t *tp);
 uint32_t stack_free(void *stack_base);
 
 // allow stack view code to show free ISR stack
-extern void *__main_stack_base__;
-extern uint32_t __main_stack_size__;
+extern uint32_t __main_stack_base__;
+extern uint32_t __main_stack_end__;
+extern uint32_t __main_thread_stack_base__;
+extern uint32_t __main_thread_stack_end__;
 
 #ifdef __cplusplus
 }