px4io: added isr_debug()

this is useful for debugging px4io internals

1 files changed, 61 insertions, 1 deletions

diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c
index c3dacb2a6..8e0e52f04 100644
--- a/apps/px4io/px4io.c
+++ b/apps/px4io/px4io.c
@@ -63,6 +63,54 @@ struct sys_state_s 	system_state;
 
 static struct hrt_call serial_dma_call;
 
+// global debug level for isr_debug()
+volatile uint8_t debug_level = 0;
+volatile uint32_t i2c_resets = 0;
+volatile uint32_t i2c_loop_resets = 0;
+
+
+/*
+  a set of debug buffers to allow us to send debug information from ISRs
+ */
+
+static volatile uint32_t msg_counter;
+static volatile uint32_t last_msg_counter;
+static volatile uint8_t msg_next_out, msg_next_in;
+#define NUM_MSG 6
+static char msg[NUM_MSG][60];
+
+/*
+  add a debug message to be printed on the console
+ */
+void isr_debug(uint8_t level, const char *fmt, ...)
+{
+	if (level > debug_level) {
+		return;
+	}
+	va_list ap;
+	va_start(ap, fmt);
+	vsnprintf(msg[msg_next_in], sizeof(msg[0]), fmt, ap);
+	va_end(ap);
+	msg_next_in = (msg_next_in+1) % NUM_MSG;
+	msg_counter++;
+}
+
+/*
+  show all pending debug messages
+ */
+static void show_debug_messages(void)
+{
+	if (msg_counter != last_msg_counter) {
+		uint32_t n = msg_counter - last_msg_counter;
+		if (n > NUM_MSG) n = NUM_MSG;
+		last_msg_counter = msg_counter;
+		while (n--) {
+			debug("%s", msg[msg_next_out]);
+			msg_next_out = (msg_next_out+1) % NUM_MSG;
+		}
+	}
+}
+
 int user_start(int argc, char *argv[])
 {
 	/* run C++ ctors before we go any further */
@@ -108,6 +156,8 @@ int user_start(int argc, char *argv[])
 	struct mallinfo minfo = mallinfo();
 	debug("free %u largest %u\n", minfo.mxordblk, minfo.fordblks);
 
+	debug("debug_level=%u\n", (unsigned)debug_level);
+
 	/* start the i2c handler */
 	i2c_init();
 
@@ -116,10 +166,20 @@ int user_start(int argc, char *argv[])
 
 	/* run the mixer at 100Hz (for now...) */
 	/* XXX we should use CONFIG_IDLE_CUSTOM and take over the idle thread instead of running two additional tasks */
+	uint8_t counter=0;
 	for (;;) {
 		poll(NULL, 0, 10);
 		perf_begin(mixer_perf);
 		mixer_tick();
 		perf_end(mixer_perf);
+		show_debug_messages();
+		if (counter++ == 200) {
+			counter = 0;
+			isr_debug(0, "tick debug=%u status=0x%x resets=%u loop_resets=%u", 
+				  (unsigned)debug_level,
+				  (unsigned)r_status_flags,
+				  (unsigned)i2c_resets,
+				  (unsigned)i2c_loop_resets);
+		}
 	}
-}
\ No newline at end of file
+}