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);

src/platforms/px4_log.h

@@ -64,6 +64,7 @@ static inline void do_nothing(int level, ...)
 #define PX4_WARN(...) 	ROS_WARN(__VA_ARGS__)
 #define PX4_INFO(...) 	ROS_WARN(__VA_ARGS__)
 #define PX4_DEBUG(...)
+#define PX4_BACKTRACE()
 
 #elif defined(__PX4_QURT)
 #include "qurt_log.h"
@@ -72,6 +73,7 @@ static inline void do_nothing(int level, ...)
  ****************************************************************************/
 #define PX4_LOG(FMT, ...) 	qurt_log(_PX4_LOG_LEVEL_ALWAYS, __FILE__, __LINE__, FMT, ##__VA_ARGS__)
 #define PX4_INFO(FMT, ...) 	qurt_log(_PX4_LOG_LEVEL_ALWAYS, __FILE__, __LINE__, FMT, ##__VA_ARGS__)
+#define PX4_BACKTRACE()
 
 #if defined(TRACE_BUILD)
 /****************************************************************************
@@ -127,8 +129,11 @@ __EXPORT extern uint64_t hrt_absolute_time(void);
 
 __EXPORT extern const char *__px4_log_level_str[5];
 __EXPORT extern int __px4_log_level_current;
+__EXPORT extern void px4_backtrace(void);
 __END_DECLS
 
+#define PX4_BACKTRACE() px4_backtrace()
+
 // __px4_log_level_current will be initialized to PX4_LOG_LEVEL_AT_RUN_TIME
 #define PX4_LOG_LEVEL_AT_RUN_TIME	_PX4_LOG_LEVEL_ERROR