Skip to content

Commit

Permalink
GCS_MAVLink: ensure payload space for ACK before sending
Browse files Browse the repository at this point in the history
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.
  • Loading branch information
peterbarker committed Jul 26, 2022
1 parent 8daacb6 commit cf915a0
Showing 1 changed file with 13 additions and 0 deletions.
13 changes: 13 additions & 0 deletions libraries/GCS_MAVLink/GCS_Common.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down

0 comments on commit cf915a0

Please sign in to comment.