Merged debug level commits from Tridge

3 files changed, 65 insertions, 12 deletions

diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c
index 235f8affa..4485daa5b 100644
--- a/apps/px4io/i2c.c
+++ b/apps/px4io/i2c.c
@@ -65,10 +65,6 @@
 #define rTRISE		REG(STM32_I2C_TRISE_OFFSET)
 
 static int		i2c_interrupt(int irq, void *context);
-#ifdef DEBUG
-static void		i2c_dump(void);
-#endif
-
 static void		i2c_rx_setup(void);
 static void		i2c_tx_setup(void);
 static void		i2c_rx_complete(void);
@@ -152,6 +148,39 @@ i2c_init(void)
 #endif
 }
 
+
+/*
+  reset the I2C bus
+  used to recover from lockups
+ */
+void i2c_reset(void)
+{
+	rCR1 |= I2C_CR1_SWRST;
+	rCR1 = 0;
+
+	/* set for DMA operation */
+	rCR2 |= I2C_CR2_ITEVFEN |I2C_CR2_ITERREN | I2C_CR2_DMAEN;
+
+	/* set the frequency value in CR2 */
+	rCR2 &= ~I2C_CR2_FREQ_MASK;
+	rCR2 |= STM32_PCLK1_FREQUENCY / 1000000;
+
+	/* set divisor and risetime for fast mode */
+	uint16_t result = STM32_PCLK1_FREQUENCY / (400000 * 25);
+	if (result < 1)
+		result = 1;
+	result = 3;
+	rCCR &= ~I2C_CCR_CCR_MASK;
+	rCCR |= I2C_CCR_DUTY | I2C_CCR_FS | result;
+	rTRISE = (uint16_t)((((STM32_PCLK1_FREQUENCY / 1000000) * 300) / 1000) + 1);
+
+	/* set our device address */
+	rOAR1 = 0x1a << 1;
+
+	/* and enable the I2C port */
+	rCR1 |= I2C_CR1_ACK | I2C_CR1_PE;
+}
+
 static int
 i2c_interrupt(int irq, FAR void *context)
 {
@@ -301,8 +330,7 @@ i2c_tx_complete(void)
 	i2c_tx_setup();
 }
 
-#ifdef DEBUG
-static void
+void
 i2c_dump(void)
 {
 	debug("CR1   0x%08x  CR2   0x%08x", rCR1,  rCR2);
@@ -310,4 +338,3 @@ i2c_dump(void)
 	debug("CCR   0x%08x  TRISE 0x%08x", rCCR,  rTRISE);
 	debug("SR1   0x%08x  SR2   0x%08x", rSR1,  rSR2);
 }
-#endif
\ No newline at end of file
diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c
index 8e0e52f04..b7e6ccb21 100644
--- a/apps/px4io/px4io.c
+++ b/apps/px4io/px4io.c
@@ -63,11 +63,12 @@ struct sys_state_s 	system_state;
 
 static struct hrt_call serial_dma_call;
 
-// global debug level for isr_debug()
+/* 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;
 
+struct hrt_call loop_overtime_call;
+
 
 /*
   a set of debug buffers to allow us to send debug information from ISRs
@@ -111,6 +112,19 @@ static void show_debug_messages(void)
 	}
 }
 
+/*
+  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 */
@@ -152,7 +166,6 @@ int user_start(int argc, char *argv[])
 		    (main_t)controls_main,
 		    NULL);
 
-
 	struct mallinfo minfo = mallinfo();
 	debug("free %u largest %u\n", minfo.mxordblk, minfo.fordblks);
 
@@ -168,6 +181,11 @@ int user_start(int argc, char *argv[])
 	/* 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();
@@ -175,10 +193,9 @@ int user_start(int argc, char *argv[])
 		show_debug_messages();
 		if (counter++ == 200) {
 			counter = 0;
-			isr_debug(0, "tick debug=%u status=0x%x resets=%u loop_resets=%u", 
+			isr_debug(1, "tick debug=%u status=0x%x resets=%u", 
 				  (unsigned)debug_level,
 				  (unsigned)r_status_flags,
-				  (unsigned)i2c_resets,
 				  (unsigned)i2c_loop_resets);
 		}
 	}
diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h
index c90db5d78..92d55a47c 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -176,3 +176,12 @@ extern int	dsm_init(const char *device);
 extern bool	dsm_input(uint16_t *values, uint16_t *num_values);
 extern int	sbus_init(const char *device);
 extern bool	sbus_input(uint16_t *values, uint16_t *num_values);
+
+/* global debug level for isr_debug() */
+extern volatile uint8_t debug_level;
+
+/* send a debug message to the console */
+extern void isr_debug(uint8_t level, const char *fmt, ...);
+
+void i2c_dump(void);
+void i2c_reset(void);