Merge branch 'px4io-i2c' into px4io-i2c-nuttx

3 files changed, 19 insertions, 8 deletions

diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h
index 4f1b067bd..a957a9e79 100644
--- a/apps/px4io/protocol.h
+++ b/apps/px4io/protocol.h
@@ -102,6 +102,7 @@
 #define PX4IO_P_STATUS_FLAGS_FMU_OK		(1 << 6) /* controls from FMU are valid */
 #define PX4IO_P_STATUS_FLAGS_RAW_PWM		(1 << 7) /* raw PWM from FMU is bypassing the mixer */
 #define PX4IO_P_STATUS_FLAGS_MIXER_OK		(1 << 8) /* mixer is OK */
+#define PX4IO_P_STATUS_FLAGS_ARM_SYNC		(1 << 9) /* the arming state between IO and FMU is in sync */
 
 #define PX4IO_P_STATUS_ALARMS			3	 /* alarm flags - alarms latch, write 1 to a bit to clear it */
 #define PX4IO_P_STATUS_ALARMS_VBATT_LOW		(1 << 0) /* VBatt is very close to regulator dropout */
diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c
index f1231b0cf..fec5eed23 100644
--- a/apps/px4io/px4io.c
+++ b/apps/px4io/px4io.c
@@ -84,7 +84,7 @@ static volatile uint8_t msg_next_out, msg_next_in;
  * output.
  */
 #define NUM_MSG 2
-static char msg[NUM_MSG][40];
+static char msg[NUM_MSG][50];
 
 /*
   add a debug message to be printed on the console
@@ -183,23 +183,23 @@ int user_start(int argc, char *argv[])
 	/* add a performance counter for mixing */
 	perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
 
-	/* run the mixer at 100Hz (for now...) */
+	/* run the mixer at ~300Hz (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;
+	uint16_t counter=0;
 	for (;;) {
 		/*
-		  if we are not scheduled for 100ms then reset the I2C bus
+		  if we are not scheduled for 10ms then reset the I2C bus
 		 */
-		hrt_call_after(&loop_overtime_call, 100000, (hrt_callout)loop_overtime, NULL);
+		hrt_call_after(&loop_overtime_call, 10000, (hrt_callout)loop_overtime, NULL);
 
-		poll(NULL, 0, 10);
+		poll(NULL, 0, 3);
 		perf_begin(mixer_perf);
 		mixer_tick();
 		perf_end(mixer_perf);
 		show_debug_messages();
-		if (counter++ == 200) {
+		if (counter++ == 800) {
 			counter = 0;
-			isr_debug(1, "tick dbg=%u stat=0x%x arm=0x%x feat=0x%x rst=%u", 
+			isr_debug(1, "d:%u stat=0x%x arm=0x%x feat=0x%x rst=%u", 
 				  (unsigned)debug_level,
 				  (unsigned)r_status_flags,
 				  (unsigned)r_setup_arming,
diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c
index be3bebada..40bf72482 100644
--- a/apps/px4io/registers.c
+++ b/apps/px4io/registers.c
@@ -270,6 +270,16 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			r_status_alarms &= ~value;
 			break;
 
+		case PX4IO_P_STATUS_FLAGS:
+			/* 
+			 * Allow FMU override of arming state (to allow in-air restores),
+			 * but only if the arming state is not in sync on the IO side.
+			 */
+			if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
+				r_status_flags = value;
+			}
+			break;
+
 		default:
 			/* just ignore writes to other registers in this page */
 			break;