From cf915a0f353176278581f8827ee7e9364c29a061 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Fri, 22 Jul 2022 15:10:13 +1000
Subject: [PATCH] GCS_MAVLink: ensure payload space for ACK before sending

Attempting to fix instance of the ACK not arriving on SITL reboot.  We already have a lot of code around trying to get this out, but there's been at least once instance it hasn't.
---
 libraries/GCS_MAVLink/GCS_Common.cpp | 13 +++++++++++++
 1 file changed, 13 insertions(+)

diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index f3384abbcd..a4e4e2987e 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -2974,6 +2974,19 @@ MAV_RESULT GCS_MAVLINK::handle_preflight_reboot(const mavlink_command_long_t &pa
         return MAV_RESULT_UNSUPPORTED;
     }
 
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    {  // autotest relies in receiving the ACK for the reboot.  Ensure
+       // there is space for it:
+        const uint32_t tstart_ms = AP_HAL::millis();
+        while (AP_HAL::millis() - tstart_ms < 1000) {
+            if (HAVE_PAYLOAD_SPACE(chan, COMMAND_ACK)) {
+                break;
+            }
+            hal.scheduler->delay(10);
+        }
+    }
+#endif
+
     // send ack before we reboot
     mavlink_msg_command_ack_send(chan, packet.command, MAV_RESULT_ACCEPTED,
                                  0, 0, 0, 0);