Finished and tested in-air restore of arming state, as long as both boards reset at the same time armings state is now retained

4 files changed, 38 insertions, 16 deletions

diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index 069028c14..b2e0c6ee4 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -279,6 +279,8 @@ PX4IO::PX4IO() :
 	_max_relays(0),
 	_max_transfer(16),	/* sensible default */
 	_update_interval(0),
+	_status(0),
+	_alarms(0),
 	_task(-1),
 	_task_should_exit(false),
 	_perf_update(perf_alloc(PC_ELAPSED, "px4io update")),
@@ -360,8 +362,8 @@ PX4IO::init()
 
 	uint16_t reg;
 
-	/* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */
-	ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &reg, sizeof(reg));
+	/* get IO's last seen FMU state */
+	ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, &reg, sizeof(reg));
 	if (ret != OK)
 		return ret;
 
@@ -392,8 +394,8 @@ PX4IO::init()
 			/* wait 10 ms */
 			usleep(10000);
 
-			/* abort after 10s */
-			if ((hrt_absolute_time() - try_start_time)/1000 > 10000) {
+			/* abort after 5s */
+			if ((hrt_absolute_time() - try_start_time)/1000 > 50000) {
 				log("failed to recover from in-air restart (1), aborting IO driver init.");
 				return 1;
 			}
@@ -432,8 +434,8 @@ PX4IO::init()
 			/* wait 10 ms */
 			usleep(10000);
 
-			/* abort after 10s */
-			if ((hrt_absolute_time() - try_start_time)/1000 > 10000) {
+			/* abort after 5s */
+			if ((hrt_absolute_time() - try_start_time)/1000 > 50000) {
 				log("failed to recover from in-air restart (2), aborting IO driver init.");
 				return 1;
 			}
@@ -770,19 +772,28 @@ PX4IO::io_handle_status(uint16_t status)
 	 */
 
 	/* check for IO reset - force it back to armed if necessary */
-	if (_status & PX4IO_P_STATUS_FLAGS_ARMED && !(status & PX4IO_P_STATUS_FLAGS_ARMED)) {
+	if (_status & PX4IO_P_STATUS_FLAGS_ARMED && !(status & PX4IO_P_STATUS_FLAGS_ARMED)
+		&& !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
 		/* set the arming flag */
-		ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARMED);
+		ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARMED | PX4IO_P_STATUS_FLAGS_ARM_SYNC);
 
 		/* set new status */
 		_status = status;
 		_status &= PX4IO_P_STATUS_FLAGS_ARMED;
+	} else if (!(_status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
+
+		/* set the sync flag */
+		ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARM_SYNC);
+		/* set new status */
+		_status = status;
+
 	} else {
 		ret = 0;
 
 		/* set new status */
 		_status = status;
 	}
+
 	return ret;
 }
 
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 96f49492f..c102bf479 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;