1 files changed, 21 insertions, 0 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();