AP_HAL_Linux: Scheduler: debug stack usage

libraries/AP_HAL_Linux/Scheduler.cpp

@@ -98,6 +98,30 @@ void Scheduler::init()
         t->thread->set_rate(t->rate);
         t->thread->start(t->name, t->policy, t->prio);
     }
+
+#if defined(DEBUG_STACK) && DEBUG_STACK
+    register_timer_process(FUNCTOR_BIND_MEMBER(&Scheduler::_debug_stack, void));
+#endif
+}
+
+void Scheduler::_debug_stack()
+{
+    uint64_t now = AP_HAL::millis64();
+
+    if (now - _last_stack_debug_msec > 5000) {
+        fprintf(stderr, "Stack Usage:\n"
+                "\ttimer = %zu\n"
+                "\tio    = %zu\n"
+                "\trcin  = %zu\n"
+                "\tuart  = %zu\n"
+                "\ttone  = %zu\n",
+                _timer_thread.get_stack_usage(),
+                _io_thread.get_stack_usage(),
+                _rcin_thread.get_stack_usage(),
+                _uart_thread.get_stack_usage(),
+                _tonealarm_thread.get_stack_usage());
+        _last_stack_debug_msec = now;
+    }
 }
 
 void Scheduler::microsleep(uint32_t usec)

libraries/AP_HAL_Linux/Scheduler.h

@@ -60,6 +60,8 @@ private:
 
     void _wait_all_threads();
 
+    void     _debug_stack();
+
     AP_HAL::Proc _delay_cb;
     uint16_t _min_delay_cb_ms;
 
@@ -102,6 +104,7 @@ private:
     bool _register_timesliced_proc(AP_HAL::MemberProc, uint8_t);
 
     uint64_t _stopped_clock_usec;
+    uint64_t _last_stack_debug_msec;
 
     Semaphore _timer_semaphore;
     Semaphore _io_semaphore;