Merged multirotor branch

1 files changed, 14 insertions, 12 deletions

diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
index bc8dfc116..e70b3fe88 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -64,11 +64,6 @@ struct sys_state_s 	system_state;
 
 static struct hrt_call serial_dma_call;
 
-#ifdef CONFIG_STM32_I2C1
-/* store i2c reset count XXX this should be a register, together with other error counters */
-volatile uint32_t i2c_loop_resets = 0;
-#endif
-
 /*
  * a set of debug buffers to allow us to send debug information from ISRs
  */
@@ -147,8 +142,10 @@ user_start(int argc, char *argv[])
 	LED_BLUE(false);
 	LED_SAFETY(false);
 
-	/* turn on servo power */
+	/* turn on servo power (if supported) */
+#ifdef POWER_SERVO
 	POWER_SERVO(true);
+#endif
 
 	/* start the safety switch handler */
 	safety_init();
@@ -159,10 +156,11 @@ user_start(int argc, char *argv[])
 	/* initialise the control inputs */
 	controls_init();
 
-#ifdef CONFIG_STM32_I2C1
-	/* start the i2c handler */
-	i2c_init();
-#endif
+	/* start the FMU interface */
+	interface_init();
+
+	/* add a performance counter for the interface */
+	perf_counter_t interface_perf = perf_alloc(PC_ELAPSED, "interface");
 
 	/* add a performance counter for mixing */
 	perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
@@ -205,6 +203,11 @@ user_start(int argc, char *argv[])
 		/* track the rate at which the loop is running */
 		perf_count(loop_perf);
 
+		/* kick the interface */
+		perf_begin(interface_perf);
+		interface_tick();
+		perf_end(interface_perf);
+
 		/* kick the mixer */
 		perf_begin(mixer_perf);
 		mixer_tick();
@@ -223,12 +226,11 @@ user_start(int argc, char *argv[])
 
 			struct mallinfo minfo = mallinfo();
 
-			isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u m=%u", 
+			isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u", 
 				  (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG],
 				  (unsigned)r_status_flags,
 				  (unsigned)r_setup_arming,
 				  (unsigned)r_setup_features,
-				  (unsigned)i2c_loop_resets,
 				  (unsigned)minfo.mxordblk);
 			last_debug_time = hrt_absolute_time();
 		}