diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index baca8fc588..7cd9661839 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -2551,8 +2551,13 @@ MAV_RESULT GCS_MAVLINK::handle_preflight_reboot(const mavlink_command_long_t &pa // flush pending parameter writes AP_Param::flush(); + // do not process incoming mavlink messages while we delay: + hal.scheduler->register_delay_callback(nullptr, 5); + + // delay to give the ACK a chance to get out, the LEDs to flash, + // the IO board safety to be forced on, the parameters to flush, ... hal.scheduler->delay(200); - + // when packet.param1 == 3 we reboot to hold in bootloader const bool hold_in_bootloader = is_equal(packet.param1, 3.0f); hal.scheduler->reboot(hold_in_bootloader);