Added backtrace function

Added PX4_BACKTRACE() to display stack trace for debugging

Added backtrace for HRT reschedule with 0 timeout

Signed-off-by: Mark Charlebois <charlebm@gmail.com>

src/platforms/posix/px4_layer/drv_hrt.c

@@ -367,6 +367,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
 	// Use this to debug busy CPU that keeps rescheduling with 0 period time
 	if (interval < HRT_INTERVAL_MIN) {
 		PX4_ERR("hrt_call_internal interval too short: %" PRIu64, interval);
+		PX4_BACKTRACE();
 	}
 
 #endif

src/platforms/posix/px4_layer/px4_log.c

@@ -1,5 +1,26 @@
+#include <stdlib.h>
 #include <px4_log.h>
+#include <execinfo.h>
 
 __EXPORT int __px4_log_level_current = PX4_LOG_LEVEL_AT_RUN_TIME;
 
 __EXPORT const char *__px4_log_level_str[_PX4_LOG_LEVEL_PANIC+1] = { "INFO", "DEBUG", "WARN", "ERROR", "PANIC" };
+
+void px4_backtrace()
+{
+	void *buffer[10];
+	char **callstack;
+	int bt_size;
+	int idx;
+
+	bt_size = backtrace(buffer, 10);
+	callstack = backtrace_symbols(buffer, bt_size);
+
+	PX4_INFO("Backtrace:", bt_size);
+
+	for(idx=0; idx < bt_size; idx++)
+		PX4_INFO("%s", callstack[idx]);
+
+	free(callstack);
+}
+

src/platforms/posix/work_queue/hrt_thread.c

@@ -165,6 +165,7 @@ static void hrt_work_process()
 
 			if (!worker) {
 				PX4_ERR("MESSED UP: worker = 0");
+				PX4_BACKTRACE();
 
 			} else {
 				worker(arg);