px4iofirmware: make forceupdate more reliable

this schedules a reboot rather than rebooting immediately, which means
the FMU gets an ACK for its reboot operation, preventing it from
timing out waiting for the ACK. That makes the timing of the reboot
more consistent, which makes it more reliable for forceupdate

3 files changed, 30 insertions, 10 deletions

diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
index 745bd5705..0b8c4a6a8 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -125,6 +125,25 @@ heartbeat_blink(void)
 	LED_BLUE(heartbeat = !heartbeat);
 }
 
+static uint64_t reboot_time;
+
+/**
+   schedule a reboot in time_delta_usec microseconds
+ */
+void schedule_reboot(uint32_t time_delta_usec)
+{
+    reboot_time = hrt_absolute_time() + time_delta_usec;
+}
+
+/**
+   check for a scheduled reboot
+ */
+static void check_reboot(void)
+{
+    if (reboot_time != 0 && hrt_absolute_time() > reboot_time) {
+        up_systemreset();
+    }
+}
 
 static void
 calculate_fw_crc(void)
@@ -249,6 +268,8 @@ user_start(int argc, char *argv[])
                     heartbeat_blink();
                 }
 
+                check_reboot();
+
 #if 0
 		/* check for debug activity */
 		show_debug_messages();
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index dea04a663..a0daa97ea 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -220,3 +220,7 @@ extern volatile uint8_t debug_level;
 
 /** send a debug message to the console */
 extern void	isr_debug(uint8_t level, const char *fmt, ...);
+
+/** schedule a reboot */
+extern void schedule_reboot(uint32_t time_delta_usec);
+
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 0358725db..ad4473073 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -518,16 +518,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			// check the magic value
 			if (value != PX4IO_REBOOT_BL_MAGIC)
 				break;
-                        
-                        // note that we don't set BL_WAIT_MAGIC in
-                        // BKP_DR1 as that is not necessary given the
-                        // timing of the forceupdate command. The
-                        // bootloader on px4io waits for enough time
-                        // anyway, and this method works with older
-                        // bootloader versions (tested with both
-                        // revision 3 and revision 4).
-
-			up_systemreset();
+
+                        // we schedule a reboot rather than rebooting
+                        // immediately to allow the IO board to ACK
+                        // the reboot command
+                        schedule_reboot(100000);
 			break;
 
 		case PX4IO_P_SETUP_DSM: