diff --git a/libraries/AP_HAL_Linux/Thread.cpp b/libraries/AP_HAL_Linux/Thread.cpp
index 2328d12627e..34aa32510c1 100644
--- a/libraries/AP_HAL_Linux/Thread.cpp
+++ b/libraries/AP_HAL_Linux/Thread.cpp
@@ -17,14 +17,19 @@
  */
 #include "Thread.h"
 
+#include <alloca.h>
 #include <sys/types.h>
+#include <stdio.h>
 #include <unistd.h>
+#include <utility>
 
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Math/AP_Math.h>
 
 #include "Scheduler.h"
 
+#define STACK_POISON 0xBEBACAFE
+
 extern const AP_HAL::HAL &hal;
 
 namespace Linux {
@@ -33,6 +38,7 @@ namespace Linux {
 void *Thread::_run_trampoline(void *arg)
 {
     Thread *thread = static_cast<Thread *>(arg);
+    thread->_poison_stack();
     thread->_run();
 
     return nullptr;
@@ -49,6 +55,79 @@ bool Thread::_run()
     return true;
 }
 
+void Thread::_poison_stack()
+{
+    pthread_attr_t attr;
+    size_t stack_size, guard_size;
+    void *stackp;
+    uint8_t *end, *start, *curr;
+    uint32_t *p;
+
+    if (pthread_getattr_np(_ctx, &attr) != 0 ||
+        pthread_attr_getstack(&attr, &stackp, &stack_size) != 0 ||
+        pthread_attr_getguardsize(&attr, &guard_size) != 0) {
+        return;
+    }
+
+    /* The stack either grows upward or downard. The guard part always
+     * protects the end */
+    end = (uint8_t *) stackp;
+    start = end + stack_size;
+    curr = (uint8_t *) alloca(sizeof(uint32_t));
+
+    /* if curr is closer to @end, the stack actually grows from low to high
+     * virtual address: this is because this function should be executing very
+     * early in the thread's life and close to the thread creation, assuming
+     * the actual stack size is greater than the guard size and the stack
+     * until now is resonably small */
+    if (abs(curr - start) > abs(curr - end)) {
+        std::swap(end, start);
+        end -= guard_size;
+
+        for (p = (uint32_t *) end; p > (uint32_t *) curr; p--) {
+            *p = STACK_POISON;
+        }
+    } else {
+        end += guard_size;
+
+        for (p = (uint32_t *) end; p < (uint32_t *) curr; p++) {
+            *p = STACK_POISON;
+        }
+    }
+
+    _stack_debug.start = (uint32_t *) start;
+    _stack_debug.end = (uint32_t *) end;
+}
+
+size_t Thread::get_stack_usage()
+{
+    uint32_t *p;
+    size_t result = 0;
+
+    /* Make sure we are tracking usage for this thread */
+    if (_stack_debug.start == 0 || _stack_debug.end == 0) {
+        return 0;
+    }
+
+    if (_stack_debug.start < _stack_debug.end) {
+        for (p = _stack_debug.end; p > _stack_debug.start; p--) {
+            if (*p != STACK_POISON) {
+                break;
+            }
+        }
+        result = p - _stack_debug.start;
+    } else {
+        for (p = _stack_debug.end; p < _stack_debug.start; p++) {
+            if (*p != STACK_POISON) {
+                break;
+            }
+        }
+        result = _stack_debug.start - p;
+    }
+
+    return result;
+}
+
 bool Thread::start(const char *name, int policy, int prio)
 {
     if (_started) {
diff --git a/libraries/AP_HAL_Linux/Thread.h b/libraries/AP_HAL_Linux/Thread.h
index 32e46407688..3c20835f9dc 100644
--- a/libraries/AP_HAL_Linux/Thread.h
+++ b/libraries/AP_HAL_Linux/Thread.h
@@ -42,6 +42,8 @@ public:
 
     bool is_current_thread();
 
+    size_t get_stack_usage();
+
 protected:
     static void *_run_trampoline(void *arg);
 
@@ -52,9 +54,16 @@ protected:
      */
     virtual bool _run();
 
+    void _poison_stack();
+
     task_t _task;
     bool _started;
     pthread_t _ctx;
+
+    struct stack_debug {
+        uint32_t *start;
+        uint32_t *end;
+    } _stack_debug;
 };
 
 class PeriodicThread : public Thread {