1 files changed, 112 insertions, 12 deletions

diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c
index 88342816e..2683ff1f6 100644
--- a/apps/px4io/px4io.c
+++ b/apps/px4io/px4io.c
@@ -37,20 +37,22 @@
  */
 
 #include <nuttx/config.h>
-#include <stdio.h>
+
+#include <stdio.h>	// required for task_create
 #include <stdbool.h>
-#include <fcntl.h>
-#include <unistd.h>
-#include <debug.h>
 #include <stdlib.h>
 #include <errno.h>
 #include <string.h>
-
-#include <nuttx/clock.h>
+#include <poll.h>
 
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_hrt.h>
 
+#include <systemlib/perf_counter.h>
+
+#include <stm32_uart.h>
+
+#define DEBUG
 #include "px4io.h"
 
 __EXPORT int user_start(int argc, char *argv[]);
@@ -59,6 +61,70 @@ extern void up_cxxinitialize(void);
 
 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_loop_resets = 0;
+
+struct hrt_call loop_overtime_call;
+
+
+/*
+  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;
+		}
+	}
+}
+
+/*
+  catch I2C lockups
+ */
+static void loop_overtime(void *arg)
+{
+	debug("RESETTING\n");
+	i2c_loop_resets++;
+	i2c_dump();
+	i2c_reset();
+	hrt_call_after(&loop_overtime_call, 50000, (hrt_callout)loop_overtime, NULL);
+}
+
+
 int user_start(int argc, char *argv[])
 {
 	/* run C++ ctors before we go any further */
@@ -66,12 +132,16 @@ int user_start(int argc, char *argv[])
 
 	/* reset all to zero */
 	memset(&system_state, 0, sizeof(system_state));
-	/* default to 50 Hz PWM outputs */
-	system_state.servo_rate = 50;
 
 	/* configure the high-resolution time/callout interface */
 	hrt_init();
 
+	/*
+	 * Poll at 1ms intervals for received bytes that have not triggered
+	 * a DMA event.
+	 */
+	hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL);
+
 	/* print some startup info */
 	lowsyslog("\nPX4IO: starting\n");
 
@@ -96,10 +166,40 @@ int user_start(int argc, char *argv[])
 		    (main_t)controls_main,
 		    NULL);
 
-
 	struct mallinfo minfo = mallinfo();
 	lowsyslog("free %u largest %u\n", minfo.mxordblk, minfo.fordblks);
 
-	/* we're done here, go run the communications loop */
-	comms_main();
-}
\ No newline at end of file
+	debug("debug_level=%u\n", (unsigned)debug_level);
+
+	/* start the i2c handler */
+	i2c_init();
+
+	/* add a performance counter for mixing */
+	perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
+
+	/* 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 (;;) {
+		/*
+		  if we are not scheduled for 100ms then reset the I2C bus
+		 */
+		hrt_call_after(&loop_overtime_call, 100000, (hrt_callout)loop_overtime, NULL);
+
+		poll(NULL, 0, 10);
+		perf_begin(mixer_perf);
+		mixer_tick();
+		perf_end(mixer_perf);
+		show_debug_messages();
+		if (counter++ == 200) {
+			counter = 0;
+			isr_debug(1, "tick debug=%u status=0x%x arming=0x%x features=0x%x resets=%u", 
+				  (unsigned)debug_level,
+				  (unsigned)r_status_flags,
+				  (unsigned)r_setup_arming,
+				  (unsigned)r_setup_features,
+				  (unsigned)i2c_loop_resets);
+		}
+	}
+}
+